Method and device for controlling robot to open door and nonvolatile storage medium

Abstract

The invention discloses a method and device for controlling a robot to open a door and a nonvolatile storage medium. The method comprises the steps that pose information corresponding to a target handle and position information corresponding to a mechanical arm and a chassis in a target robot are obtained; inputting the pose information, the position information corresponding to the mechanical arm and the position information corresponding to the chassis into the target dynamic motion primitive model to obtain a predicted motion track; state information corresponding to the mechanical arm and the chassis is obtained; based on the state information corresponding to the mechanical arm and the chassis, corresponding dynamic models are established; an action sequence is generated based on the predicted motion trail, kinetic models corresponding to the mechanical arm and the chassis and a prediction control model based on the model; and controlling the mechanical arm and the chassis to perform door opening motion based on the action sequence. The technical problems that a mechanical arm and a chassis of an existing robot are poor in coordination, a handle can not be accurately grabbed or the door opening action can not be completed when the robot moves according to a fixed track, and accidents are likely to happen are solved.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a method and device for controlling a robot to open a door, and a non-volatile storage medium. Background Art

[0002] At present, robots have poor environmental adaptability. Once the actual position and posture of the door deviate from the preset situation, the robot arm may not be able to accurately grasp the door handle or open the door according to the fixed trajectory, which may easily lead to collision. Lack of flexibility. For different types of door handles, such as different screwing directions, or different types of doors, such as revolving doors and sliding doors, the pre-programmed trajectory is difficult to be universal and needs to be reprogrammed for each door type. It completely relies on pre-set fixed trajectories, without considering the uncertainty in the actual environment and the diversity of doors, and cannot adjust the movement of the robot arm in real time according to environmental changes.

[0003] To address the above-mentioned problems, no effective solution has been proposed yet. Summary of the invention

[0004] The embodiments of the present invention provide a method, device and non-volatile storage medium for controlling a robot to open a door, so as to at least solve the technical problems that the current robot's mechanical arm and chassis have poor coordination, may not be able to accurately grasp the handle or complete the door opening action when moving along a fixed trajectory, and may easily cause accidents.

[0005] According to one aspect of an embodiment of the present invention, there is provided a method for controlling a robot to open a door, comprising: obtaining posture information corresponding to a target handle and position information corresponding to a robotic arm and a chassis in a target robot; inputting the posture information and the position information corresponding to the robotic arm and the chassis into a target dynamic motion primitive model to obtain an expected motion trajectory; obtaining state information corresponding to the robotic arm and the chassis, wherein the state information includes a motion speed; establishing a dynamic model corresponding to the robotic arm and the chassis based on the state information corresponding to the robotic arm and the chassis; generating an action sequence based on the expected motion trajectory, the dynamic models corresponding to the robotic arm and the chassis, and a model-based predictive control model; and controlling the robotic arm and the chassis to perform a door opening movement based on the action sequence.

[0006] Optionally, obtaining the posture information corresponding to the target handle includes: obtaining image data of the target handle; inputting the image data into a preset target recognition model to obtain edge information of the target handle; determining a depth image corresponding to the image data; determining a depth value corresponding to the target handle according to the depth image; and determining the posture information of the target handle based on the edge information and the depth value.

[0007] Optionally, before inputting the posture information, the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory, it includes: obtaining multiple teaching trajectories corresponding to each of the multiple handle types; using the multiple teaching trajectories corresponding to each of the multiple handle types to train the preset original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0008] Optionally, a preset original dynamic motion primitive model is trained using multiple teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: calculating the smoothness of each of the multiple teaching trajectories corresponding to each of the multiple handle types; selecting a teaching trajectory whose smoothness meets a preset condition among the multiple teaching trajectories corresponding to each of the multiple handle types as a target teaching trajectory to obtain the target teaching trajectory corresponding to each of the multiple handle types; based on the target teaching trajectories corresponding to each of the multiple handle types, training the original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0009] Optionally, the preset original dynamic motion primitive model is trained using the teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: based on a Gaussian mixture model, extracting feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, wherein the feature points represent the key position of the handle of the corresponding handle type during the screwing process; based on the Gaussian mixture regression model and the feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, adjusting the parameters in the original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0010] Optionally, the posture information, the position information corresponding to the robotic arm and the chassis are input into the target dynamic motion primitive model to obtain an expected motion trajectory, including: determining the target handle type of the target handle based on the posture information; determining the starting point information and end point information of the handle rotation corresponding to the target handle type; inputting the starting point information, end point information, posture information, and the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory.

[0011] Optionally, based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model, an action sequence is generated, including: defining a cost function, wherein the cost function is used to evaluate the performance of the predicted action sequence; determining the constraints corresponding to the robot arm and the chassis based on the performance parameters corresponding to the robot arm and the chassis; integrating the dynamic models corresponding to the robot arm and the chassis into the model-based predictive control model to obtain a target predictive control model; based on the predicted motion trajectory, the cost function, and the constraints corresponding to the robot arm and the chassis, the target predictive control model is used to generate an action sequence.

[0012] According to another aspect of an embodiment of the present invention, there is also provided a device for controlling a robot to open a door, comprising: a first acquisition module, for acquiring posture information corresponding to a target handle and position information corresponding to a robotic arm and a chassis in a target robot; an input module, for inputting the posture information and the position information corresponding to the robotic arm and the chassis into a target dynamic motion primitive model to obtain an expected motion trajectory; a second acquisition module, for acquiring state information corresponding to the robotic arm and the chassis, wherein the state information includes a motion speed; an establishment module, for establishing a dynamic model corresponding to the robotic arm and the chassis based on the state information corresponding to the robotic arm and the chassis; a generation module, for generating an action sequence based on the expected motion trajectory, the dynamic models corresponding to the robotic arm and the chassis, and a model-based predictive control model; and a control module, for controlling the robotic arm and the chassis to perform a door opening movement based on the action sequence.

[0013] According to another aspect of an embodiment of the present invention, a non-volatile storage medium is provided, the non-volatile storage medium including a stored program, wherein when the program is running, the device where the non-volatile storage medium is located is controlled to execute any one of the above-mentioned methods for controlling the robot to open the door.

[0014] According to yet another aspect of an embodiment of the present invention, a computer device is provided. The computer device includes a processor, and the processor is used to run a program. When the program is run, any one of the above methods for controlling a robot to open a door is executed.

[0015] According to yet another aspect of an embodiment of the present invention, there is further provided a computer program product, including a computer program, which implements any one of the above methods for controlling a robot to open a door when the computer program is executed by a processor.

[0016] In an embodiment of the present invention, a method for controlling a robot to open a door is adopted, by obtaining posture information corresponding to a target handle and position information corresponding to a mechanical arm and a chassis of a target robot; inputting the posture information and the position information corresponding to the mechanical arm and the chassis into a target dynamic motion primitive model to obtain an expected motion trajectory; obtaining state information corresponding to the mechanical arm and the chassis, wherein the state information includes a motion speed; establishing a dynamic model corresponding to the mechanical arm and the chassis based on the state information corresponding to the mechanical arm and the chassis; generating an action sequence based on the expected motion trajectory, the dynamic models corresponding to the mechanical arm and the chassis, and a predictive control model based on the model; based on the action sequence, controlling the mechanical arm and the chassis to perform a door opening motion, thereby achieving the purpose of generating an action sequence based on the expected motion trajectory to control the mechanical arm and the chassis, thereby realizing the technical effect of improving the flexibility of the robot and reducing the probability of accidents, thereby solving the technical problem that the mechanical arm and the chassis of the current robot have poor coordination, may not be able to accurately grasp the handle or complete the door opening action when moving along a fixed trajectory, and is prone to accidents. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0018] Figure 1 A hardware structure block diagram of a computer terminal for implementing a method for controlling a robot to open a door is shown;

[0019] Figure 2 is a flow chart of a method for controlling a robot to open a door according to an embodiment of the present invention;

[0020] Figure 3 is a block diagram of learning and generalizing a robot arm door opening trajectory based on DMP according to an optional embodiment of the present invention;

[0021] Figure 4 is a schematic diagram of a trajectory of a method for controlling a robot to open a door provided in an optional embodiment of the present invention;

[0022] Figure 5 is a MPC framework diagram provided according to an optional embodiment of the present invention;

[0023] Figure 6 is a system schematic diagram of a method for controlling a robot to open a door according to an optional embodiment of the present invention;

[0024] Figure 7 4 is a structural block diagram of a device for controlling a robot to open a door according to an embodiment of the present invention. DETAILED DESCRIPTION

[0025] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0026] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0027] First, some nouns or terms that appear in the description of the embodiments of the present application are subject to the following interpretations:

[0028] DMP (Dynamic Movement Primitives) is an algorithmic framework for generating and replicating complex motion trajectories, which is widely used in the fields of robotics, motion control, and machine learning. The basic idea of ​​DMP is to decompose complex motion patterns into a set of simple primitives that can be learned and generalized, so that robots can perform dynamic motion tasks efficiently and adjust their motion to adapt to changes even when faced with new environmental conditions or targets.

[0029] MPC (Model Predictive Control) is an advanced model-based control algorithm suitable for complex systems with multiple inputs and multiple outputs (MIMO), constraints, and the need to optimize and predict future behaviors. The core idea of ​​MPC is to solve the optimal control sequence within a finite time range at each sampling time point, while considering the system model, predicting the future system state, and constraints.

[0030] According to an embodiment of the present invention, a method embodiment of a method for controlling a robot to open a door is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0031] The method embodiment provided in the first embodiment of the present application can be executed in a mobile terminal, a computer terminal or a similar computing device. Figure 1 The hardware structure block diagram of a computer terminal for implementing a method for controlling a robot to open a door is shown. Figure 1As shown, the computer terminal 10 may include one or more (102a, 102b, ..., 102n are used to illustrate) processors (the processor may include but is not limited to a processing device such as a microprocessor MCU or a programmable logic device FPGA), a memory 104 for storing data. In addition, it may also include: a display, an input / output interface (I / O interface), a universal serial bus (USB) port (which may be included as one of the ports of the BUS bus), a network interface, a power supply and / or a camera. It can be understood by those skilled in the art that Figure 1 The structure shown is only for illustration and does not limit the structure of the above electronic device. Figure 1 More or fewer components as shown, or with Figure 1 Different configurations shown.

[0032] It should be noted that the one or more processors and / or other data processing circuits described above may generally be referred to herein as "data processing circuits". The data processing circuits may be embodied in whole or in part as software, hardware, firmware, or any other combination thereof. In addition, the data processing circuit may be a single independent processing module, or may be incorporated in whole or in part into any of the other components in the computer terminal 10. As described in the embodiments of the present application, the data processing circuit acts as a processor control (e.g., selection of a variable resistor terminal path connected to an interface).

[0033] The memory 104 can be used to store software programs and modules of application software, such as program instructions / data storage devices corresponding to the method for controlling the robot to open the door in the embodiment of the present invention. The processor executes various functional applications and data processing by running the software programs and modules stored in the memory 104, that is, the method for controlling the robot to open the door of the above-mentioned application program is realized. The memory 104 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 104 may further include a memory remotely arranged relative to the processor, and these remote memories may be connected to the computer terminal 10 via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0034] The display may be, for example, a touch screen liquid crystal display (LCD) that enables a user to interact with a user interface of the computer terminal 10 .

[0035] Figure 2 is a flow chart of a method for controlling a robot to open a door according to an embodiment of the present invention. Figure 2 As shown, the method comprises the following steps:

[0036] Step S202, obtaining the position information corresponding to the target handle and the position information corresponding to the robot arm and chassis in the target robot.

[0037] In this step, the pose information corresponding to the target handle and the position information corresponding to the manipulator and chassis in the target robot are obtained, which usually involves the collection and processing of sensor data and the estimation of the robot state. Robots are usually equipped with visual sensors, such as RGB-D cameras or depth cameras, for environmental perception and target recognition. To obtain the pose information of the target handle, you can first obtain image data of the environment from the visual sensor. You can use target detection algorithms (such as YOLOv5, Mask R-CNN, etc.) to process the image and identify the area of ​​the target handle. This step may require a pre-trained deep learning model to accurately identify different types of door handles. After identifying the target handle, it is necessary to further estimate its position and pose in three-dimensional space. This is usually achieved through depth information and camera calibration parameters. The pose information obtained includes the three-dimensional coordinates (x, y, z) of the door handle, rotation angle or quaternion to describe its orientation.

[0038] After obtaining the position information of the manipulator arm and chassis of the target robot, the target pose that the manipulator end effector needs to reach and the target position that the chassis needs to reach are defined according to the task requirements (such as turning the door handle to unlock, pushing the door, etc.) to ensure that the robot can effectively interact with the door handle. Among them, the position information, including the manipulator arm joint angle, can be obtained by using the rotary encoder installed on each joint to read the current angle of the manipulator arm joint in real time, so as to know the current position and pose of the manipulator end effector (such as the gripper). To obtain the chassis position and orientation, the sensors installed on the chassis, such as IMU (inertial measurement unit), wheel speed encoder or laser radar, can be used to estimate the current position (x, y) and orientation angle (θ) of the chassis. The target handle pose information obtained by the visual sensor can be fused with the robot joint angle, chassis position and other information to form a complete robot state representation. This may involve coordinate transformation, converting the handle pose information from the camera coordinate system to the robot world coordinate system.

[0039] Through the above steps, the robot can automatically identify the door handle in the environment and estimate the current position of the robot arm and chassis, providing necessary information for subsequent motion planning and control. In the autonomous door opening task, this information will be used to generate the motion trajectory of the robot arm to turn the door handle, and to achieve coordinated control of the chassis and the robot arm based on MPC to ensure that the robot can perform the door opening operation accurately and stably.

[0040] Step S204, inputting the posture information, the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory.

[0041] In this step, the posture information, the position information corresponding to the manipulator and the chassis are input into the target dynamic motion primitive (DMP, Dynamic Movement Primitives) model to generate the expected motion trajectory. The DMP model can be defined first. For the manipulator twisting the door handle, the DMP model may contain multiple Gaussian kernel functions, which will be used to generate the forcing function of the twisting action. For the movement of the chassis, the DMP may focus on the primitives of linear or curved motion. The acquired posture information (including position and posture) of the door handle is used as the target position of the DMP. The current joint angle, velocity and acceleration of the manipulator, as well as the current position, velocity and orientation angle of the chassis are input as the starting state of the DMP. This information is crucial for generating the trajectory from the current state to the target state. Using the differential equation of the DMP model, the starting state, target position and forcing function parameters are substituted, and the expected motion trajectory of the manipulator and chassis is dynamically calculated by numerical integration or analytical integration. This includes a sequence of position, velocity and acceleration. For the robot arm, the calculation process involves the motion curve from the current joint angle to the target position, while taking into account the action of turning the door handle. For the chassis, the optimal path from the current position to a position near the door handle is calculated to ensure that the robot arm can perform the task smoothly.

[0042] The DMP model enables the arm and chassis to generate smooth and adaptable motion trajectories, even when task conditions change (such as a slight change in the position of a door handle), by adjusting the parameters of the forcing function to accommodate these changes. This approach is particularly useful for robotic operations that require high flexibility and adaptive control, such as turning different types of door handles or moving in complex environments. The dynamic generation capabilities of DMP allow the arm and chassis to efficiently complete tasks while maintaining coordination and consistency.

[0043] Step S206, obtaining status information corresponding to the robot arm and the chassis respectively, wherein the status information includes movement speed.

[0044] In this step, obtaining the state information corresponding to the manipulator and chassis, including their movement speed, is the basis for achieving precise control and coordinated motion of the robot. These state information are usually measured and estimated by sensors and encoders installed on the robot. Among them, the state information can include joint angles. Each joint of the manipulator is usually equipped with a rotary encoder that can measure the current joint angle in real time. These data provide the current configuration information of the manipulator. Joint speed, by analyzing the rate of change of the joint angle over time, the movement speed of the joint can be obtained. Some encoders have the ability to measure speed themselves, while others need to be calculated from angle data through techniques such as differential or digital filtering. Joint acceleration, joint acceleration can be estimated by differential or quadratic differential of joint speed. This helps to predict the future motion state of the manipulator. Torque sensor data, the torque sensor of the manipulator can provide information on joint loads, which is very important for adjusting the control strategy and optimizing motion planning. Wheel speed encoder data, the wheel speed encoder of the chassis can provide information on the rotation speed and displacement of the wheel, through which the linear velocity and angular velocity of the chassis can be estimated. IMU (Inertial Measurement Unit) data, IMU can measure the chassis' linear acceleration, angular acceleration, angular velocity, and tilt angle. Combined with wheel speed encoder data, algorithms such as Kalman filters can be used to accurately estimate the chassis' speed and position.

[0045] By acquiring and integrating the state information of the manipulator and chassis in real time, including their movement speed, the robot control system can accurately plan the next movement according to the current state and task requirements, and adjust the control strategy to ensure that the robot can perform tasks safely and efficiently in complex environments. In the context of the autonomous door opening control system, this state information is crucial to achieve coordinated control of the manipulator and chassis.

[0046] Step S208: establishing dynamic models corresponding to the robot arm and the chassis respectively based on the state information corresponding to the robot arm and the chassis respectively.

[0047] In this step, based on the state information corresponding to each of the manipulator and chassis, their respective dynamic models can be established. The dynamic model describes the change in the motion state of an object or system after being subjected to force. For robot control, this means understanding and predicting the response of the manipulator joint and chassis under the action of external forces or torques. The dynamic model of the manipulator is usually based on Newton-Euler or Lagrangian dynamics methods, which describes the relationship between the torque at the joint and the joint angle, velocity and acceleration when the end effector of the manipulator moves. The current angle, velocity and acceleration of each joint, as well as their changes over time, can be obtained from sensors (such as rotary encoders). Based on the structural parameters of the manipulator (such as mass, inertia, joint position, etc.) and the motion state (joint angle, velocity and acceleration), the dynamic formula is used to calculate the torque required for each joint. This usually involves complex matrix operations and differential equation solving. The above dynamic relationship is parameterized for easy use of the control algorithm. For example, the joint torque can be expressed as a function of the joint angle, velocity and acceleration. For the chassis of the robot, the dynamic model usually focuses on the relationship between the displacement, velocity, acceleration and the applied force or torque of the chassis, which is especially applicable to the differential chassis. Define the state variables of the chassis, including the position (x, y) and orientation angle (θ) of the chassis in the world coordinate system, as well as their derivatives (velocity and angular velocity). Based on the physical properties of the chassis (such as mass, friction, wheel size, etc.) and control inputs (such as the driving force of the left and right wheels), establish the kinematic and dynamic models of the chassis. For the differential chassis, its model is usually based on the kinematic relationship of the wheels to describe the changes in the position and orientation of the chassis. The physical constraints of the chassis, such as the maximum speed, maximum angular velocity and turning radius, can also be considered. These constraints will be incorporated into the dynamic model to ensure the feasibility and safety of control.

[0048] After mastering the individual dynamic models of the manipulator and chassis, they can be further integrated to form a joint dynamic model for predicting and controlling the overall behavior of the robot when performing complex tasks such as opening a door. This integration takes into account the interaction between the manipulator and the chassis, such as the stability of the chassis when the manipulator is operating, and the dynamic response of the manipulator when the chassis moves.

[0049] Specifically, the state of the chassis is uniquely determined by the coordinates and orientation. The state variables of the chassis can be set as where x mr ,y mr , Represent the position and orientation of the mobile chassis in the world coordinate system. The control input of the chassis is U mr =[v,ω], where v and ω represent the forward speed and rotational speed of the mobile chassis respectively. The state space equation of the chassis is The dynamic model of the chassis is

[0050]

[0051] The state variable of the robot arm is the joint angle of the robot arm, X arm =q∈R 6 , the system input of the robot arm is the angular velocity of the robot arm joint, The state equation of the robot arm is The dynamic model of the robot arm is

[0052] Combining the state space models of the chassis and the manipulator, the dynamic model of the mobile manipulator is described by a discrete-time nonlinear system: k+1 =f(X k, U k ), where the state variable of the system is X = [X mr ,X arm ] T ∈R 9 , system input

[0053] Enter U=[U mr ,U arm ] T , the mobile robot system model is

[0054] The integrated dynamics model can serve as the basis for the MPC (model predictive control) algorithm, which can achieve coordinated motion of the robot arm and chassis by predicting future states and optimizing control inputs, thereby completing the predetermined robot task. This model prediction and coordinated control is essential for the robot's autonomous operation in complex environments and can improve the accuracy and robustness of its motion.

[0055] Step S210, generating an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model.

[0056] In this step, the control action sequence is generated according to the predicted motion trajectory, the dynamic models of the manipulator and chassis, and the model-based predictive control (MPC) method. First, the position, velocity, and acceleration targets in the predicted motion trajectory are converted into input forms suitable for the dynamic models of the manipulator and chassis. For the manipulator, the desired position of the end effector is first converted into joint angles through inverse kinematics, and then the required joint angular velocity is calculated through the discretized manipulator dynamic model; for the chassis, the desired displacement and orientation targets are converted into linear velocity and angular velocity control signals. The MPC model is established, the prediction horizon and control horizon are defined, and the cost function and constraints are set. The cost function usually includes tracking error, control energy, and rate of change, while the constraints may include joint angle range, torque limit, chassis speed, and steering angle. At each sampling time point of the MPC, the dynamic models of the manipulator and chassis are used to predict how the system will change within the future prediction horizon according to different control input sequences. This includes predicting the position, velocity and acceleration of the end effector of the robot arm, as well as the position and orientation of the chassis. Based on the prediction results and the set cost function, the MPC algorithm will solve an optimization problem to find the control sequence that minimizes the total cost within the future control horizon, that is, the action sequence of the robot.

[0057] Among them, the MPC algorithm only applies the first control input in the control sequence to the robot and chassis, and then executes the prediction and optimization steps again, forming a rolling optimization process. In this way, the system can adjust the control strategy in real time to cope with uncertainties and changes in the motion process. During the execution process, the actual state of the robot and chassis is monitored in real time by sensors, compared with the predicted state, and the parameters or constraints of the MPC model are adjusted according to the deviation to improve the accuracy and robustness of the control.

[0058] Through this series of steps, MPC is able to generate an optimized action sequence that not only takes into account the current target trajectory, but also predicts future behavior based on the dynamic model, and ensures the coordinated movement of the robot arm and chassis when performing tasks by optimizing control inputs in real time. This approach is particularly suitable for robotic tasks that require high-precision control and real-time adaptation to environmental changes, such as moving on uneven terrain, grasping and manipulating objects. In the scenario of autonomous door opening, MPC can ensure that the robot arm accurately turns the door handle, while the chassis stably moves the robot arm to the appropriate position to complete the door opening task.

[0059] Step S212, based on the action sequence, control the robotic arm and chassis to perform door opening movement.

[0060] In this step, the robot arm and chassis are controlled to perform door opening motion based on the pre-generated action sequence. The generated action sequence can be converted into target values ​​of the robot arm joint position and chassis posture, which usually involves inverse kinematics and inverse dynamics calculations to convert the target position into control signals. Then a real-time control algorithm such as PID (proportional integral differential) control, sliding mode control or adaptive control can be used to ensure that the robot arm and chassis can accurately track the predefined trajectory.

[0061] Through the above steps, the purpose of generating an action sequence based on the expected motion trajectory to control the robotic arm and chassis can be achieved, thereby realizing the technical effect of improving the flexibility of the robot and reducing the probability of accidents, and further solving the technical problem that the robotic arm and chassis of the current robot have poor coordination, and may not be able to accurately grasp the handle or complete the door opening action when moving along a fixed trajectory, which may easily lead to accidents.

[0062] As an optional embodiment, obtaining posture information corresponding to a target handle includes: obtaining image data of the target handle; inputting the image data into a preset target recognition model to obtain edge information of the target handle; determining a depth image corresponding to the image data; determining a depth value corresponding to the target handle based on the depth image; and determining the posture information of the target handle based on the edge information and the depth value.

[0063] Optionally, image data can be obtained through a depth camera installed on a mobile manipulator, and input into a trained YOLOv5 model for processing. The model outputs information such as the bounding box coordinates of the detected door and door handle in the image, so as to determine their approximate position and outline. Next, a depth image of the corresponding image is obtained, from which the depth value of the previously detected door handle position is obtained. At the same time, the camera intrinsic and extrinsic parameters can be obtained in advance through camera calibration. Then, based on the camera intrinsic parameters and depth values, the two-dimensional pixel coordinates of the door handle obtained by YOLOv5 are converted into three-dimensional coordinates in the camera coordinate system. Then, using the camera extrinsic parameters, it is further converted to the world coordinate system to determine its three-dimensional position coordinates.

[0064] As an optional embodiment, before inputting the posture information, the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory, it includes: obtaining multiple teaching trajectories corresponding to each of the multiple handle types; using the multiple teaching trajectories corresponding to each of the multiple handle types to train the preset original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0065] Optionally, before inputting the pose information, the position information of the robot arm and the chassis into the target dynamic motion primitive (DMP) model to obtain the expected motion trajectory, a series of training and preparation steps are required to ensure that the DMP model can generalize and adapt to a variety of door handle types.

[0066] The teaching trajectory data of different door handle types (such as round knobs, lever handles, push-pull handles, etc.) can be obtained through manual teaching or robot autonomous learning. The teaching process involves manually guiding the robot to the target handle, performing grasping and door opening actions, and recording data such as the position, velocity, and acceleration of the robot end effector in three-dimensional space. In order to improve the robustness and adaptability of the model, multiple different teaching trajectories should be collected for each door handle type, covering different starting positions, door opening directions, and speeds. The DMP model usually contains a set of basic motion patterns and a set of parameters, which can describe and generate complex motion trajectories. Before model training, basic parameters such as the center position, target position, time constant, and acceleration weight of the DMP need to be set. These parameters define the basic motion characteristics of the DMP. Using the collected teaching trajectory data of different door handle types, the DMP model is trained to learn the motion patterns corresponding to different handle types. After training, the parameters of the original DMP model will be adjusted to adapt to the motion characteristics of multiple door handle types, thereby generating a target DMP model. The model will be able to generate an estimated motion trajectory adapted to a specific handle type based on the input door handle pose information and the current state of the robotic arm.

[0067] In summary, by collecting and utilizing the teaching trajectory data of different door handle types, training and adjusting the parameters of the original DMP model, a target DMP model can be generated, which can generalize and generate motion trajectories that adapt to a variety of handle types. This improves the flexibility and adaptability of the robot to autonomously open doors in different environments.

[0068] As an optional embodiment, a preset original dynamic motion primitive model is trained using multiple teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: calculating the smoothness of each of the multiple teaching trajectories corresponding to each of the multiple handle types; selecting a teaching trajectory whose smoothness meets a preset condition among the multiple teaching trajectories corresponding to each of the multiple handle types as a target teaching trajectory to obtain the target teaching trajectory corresponding to each of the multiple handle types; based on the target teaching trajectories corresponding to each of the multiple handle types, training the original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0069] Optionally, a preset original dynamic motion primitive (DMP) model is trained using multiple teaching trajectories corresponding to each of the multiple handle types to obtain a more generalized and smoother target dynamic motion primitive model. Smoothness is an indicator of trajectory continuity and motion smoothness. For the DMP model, a smooth trajectory can be better learned and reproduced by the model. The smoothness calculation method usually involves the first or second order derivatives between trajectory points, such as the rate of change of joint velocity and acceleration. A threshold or standard for smoothness can be given, and those trajectories with the highest smoothness can be selected from the teaching trajectories collected for each handle type as the target teaching trajectories. These target teaching trajectories will be used for subsequent DMP model training to generate smoother and more natural motion trajectories.

[0070] In addition to selecting the target teaching trajectory by smoothness, the target teaching trajectory can also be obtained by fitting multiple teaching trajectories. Figure 3 is a block diagram of learning and generalizing the robot door opening trajectory based on DMP according to an optional embodiment of the present invention, such as Figure 3 As shown, first, multiple teaching trajectories can be preprocessed, because multiple teaching trajectories will deviate when reproduced in a certain time series. The dynamic time warping (DTW) method can be used to optimize the alignment of the trajectory sequence. DTW can characterize the similarity between two trajectory sequences and obtain a method for aligning two sequences. First, the distance matrix between each point of the two trajectory sequences is calculated, and then a path from the upper left corner to the lower right corner of the matrix is ​​found so that the sum of the elements on the path is minimized. Specific implementation: Given two trajectory sequences A = [a1, a2, ..., an] and B = [b1, b2, ..., bm], the distance matrix D between them is calculated using the Euclidean distance, where D(i, j) represents the distance between the i-th point in sequence A and the j-th point in sequence B, that is, D(i, j) = ‖ai-dj‖. Next, we construct a cumulative distance matrix M based on the distance matrix, M(i,j)=D(i,j)+min{M(i-1,j),M(i,j-1),M(i-1,j-1)}. After filling the cumulative distance matrix M, we backtrack from the lower right corner M(n,m) and return to the upper left corner M(1,1) along the direction of the minimum cumulative distance to find the optimal alignment path from the upper left corner to the lower right corner.

[0071] After finding the optimal alignment path from the upper left corner to the lower right corner, trajectory fitting can be performed, the purpose of which is to convert the aligned trajectory data into a form suitable for the dynamic motion primitive (DMP) model. Valid trajectory points can be extracted from the alignment path, which usually contain information such as the position, velocity, and acceleration of the end effector of the robot arm. The trajectory points are then preprocessed, such as smoothing, filtering, or data regularization, to remove noise or outliers and ensure the continuity and smoothness of the trajectory. The processed trajectory points are parameterized, that is, the trajectory data is described by a mathematical function or model. For the DMP model, this involves converting the trajectory points into a form in the DMP parameter space. In the DMP model, the trajectory can be described by a combination of a central attraction potential function and a Gaussian kernel function. The central attraction potential function defines the motion trend from the starting point to the target point, while the Gaussian kernel function defines the deviation and restoring force during the motion process, helping to generate a smooth motion trajectory. The aligned trajectory data is used to fit the parameters in the DMP model, including the parameters of the central attraction potential function, the weight and standard deviation of the Gaussian kernel function, etc. This fitting process can be performed through machine learning algorithms, such as local weighted regression (LWR), to find the parameter combination that can best match the trajectory generated by the DMP model with the taught trajectory. The fitted DMP parameters are applied to the model to generate a new motion trajectory. This trajectory will combine the current position and speed of the robot arm and the position information of the target point, solve the differential equation, gradually integrate the position and speed, and finally generate a smooth and natural motion path from the current position to the target position, that is, the target taught trajectory.

[0072] As an optional embodiment, a preset original dynamic motion primitive model is trained using teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: based on a Gaussian mixture model, extracting feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, wherein the feature points represent the key position of the handle of the corresponding handle type during the screwing process; based on a Gaussian mixture regression model and the feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, adjusting the parameters in the original dynamic motion primitive model to obtain a target dynamic motion primitive model.

[0073] Optionally, a Gaussian mixture model (GMM) can be used to model the target teaching trajectory and extract statistical features, i.e., feature points, from the teaching data. The Gaussian mixture model is a probabilistic model used to describe complex data distributions. It is composed of a linear combination of multiple Gaussian distributions. In this scenario, GMM is used to extract feature points related to a specific handle type in the teaching trajectory. These feature points characterize key positions in the screwing process, such as the grip point, the start point of rotation, and the end point of rotation. Through GMM, the statistical characteristics of the teaching trajectory data can be analyzed to identify features such as joint angles, velocities, and accelerations related to the handle type and screwing action. GMM can fit the dynamic characteristics of the teaching trajectory, including joint angles, velocities, and accelerations. First, for the target teaching trajectory of each handle type, the parameters of GMM, including the mean, variance, and mixing weights of the Gaussian kernel, are fitted using methods such as maximum likelihood estimation. Based on the parameters of GMM, Gaussian mixture regression (GMR) can be used to generate new motion trajectories from GMM. Gaussian mixture regression is a nonlinear regression method that uses the parameters of GMM to map from input data to output data. The input data here can be time or the starting position of the robot arm, while the output data is the joint angle, velocity and acceleration of the robot arm during the twisting process. GMR maps input (such as time, starting position) to output (such as joint angle, velocity and acceleration) according to the correlation in the teaching data to generate motion trajectories. The smooth trajectory generated by GMR is used as the input of DMP model training, and the parameters of the DMP model, such as the parameters of the central attraction potential function and the weight of the Gaussian kernel function, are adjusted so that the DMP model can learn and reproduce these smooth trajectories. During or after the training process, the generated target dynamic motion primitive model can also be verified. The performance of the model can be evaluated by simulating or experimentally executing the motion trajectory generated by the new model to ensure that it can adapt to different handle types and generate smooth motion trajectories. If the model performance does not meet expectations, it may be necessary to adjust the parameter settings during the training process and iterate the training to further optimize the model.

[0074] Specifically, the collected teaching trajectory data under different handle types are input into GMM for training. GMM can learn the distribution characteristics of different twisting actions. Each Gaussian kernel represents a statistical description of a motion pattern. The parameters such as the mean, variance and weight of the Gaussian kernel reflect the center position, range of variation and importance of the pattern. By analyzing the parameters of each Gaussian kernel in GMM, the feature points directly related to the twisting action stage are identified. For example, points where the joint angle changes significantly can be found from the mean of the Gaussian kernel, which may correspond to key positions in the process of grasping the door handle and twisting. According to the extracted feature points, an input-output mapping relationship is constructed, where the input can be a time series or the starting position of the robot arm, and the output is the feature points such as joint angle, velocity and acceleration related to the twisting process. Using the constructed input-output mapping relationship, the parameters in the DMP model are adjusted through the GMR algorithm. GMR uses the Gaussian kernel parameters of GMM to map the input to the output through linear combination to generate a new motion trajectory. In the DMP model, this usually involves adjusting parameters such as the central attraction potential function and the weight of the Gaussian kernel function, so that the DMP can adapt to different handle types and twisting actions. Based on GMM and GMR, the DMP model is trained using the adjusted parameters so that the model can learn the commonalities and differences of twisting actions under different handle types. Through multiple iterations and optimizations, the DMP model gradually learns to generate smooth and natural motion trajectories adapted to specific twisting actions based on inputs (such as handle type, starting position, etc.).

[0075] Through the above steps, we can effectively use Gaussian mixture model and Gaussian mixture regression, analyze and learn the teaching trajectory under multiple handle types, adjust the key parameters in the original dynamic motion primitive model, and generate a more powerful and generalized target dynamic motion primitive model. This model can not only reproduce the actions in the teaching trajectory, but also automatically generate the corresponding twisting motion trajectory when encountering a new handle type, greatly improving the robot's operational flexibility and autonomy in complex environments.

[0076] Through the above steps, the information of the smooth teaching trajectory corresponding to multiple handle types can be integrated into the DMP model to generate a target DMP model that can be generalized to multiple handle types and can generate smooth and natural motion trajectories. Such a model not only improves the fluency and efficiency of the robot's door opening action, but also enhances its adaptability and robustness in complex environments.

[0077] As an optional embodiment, the posture information, the position information corresponding to the robot arm and the chassis are input into the target dynamic motion primitive model to obtain the expected motion trajectory, including: determining the target handle type of the target handle based on the posture information; determining the starting point information and end point information of the handle rotation corresponding to the target handle type; inputting the starting point information, end point information, posture information and the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory.

[0078] Optionally, the start and end points of the handle rotation action can be determined based on the identified handle type and the posture information obtained during visual recognition. The start point usually corresponds to the position where the end effector of the robot arm grasps the handle, while the end point corresponds to the target position where the door is opened after the twisting action or the push-pull action is completed. This information is essential for generating an accurate motion trajectory. Then the start point information, end point information, posture information, and the position information of the robot arm and chassis are input into the target DMP model to generate an expected motion trajectory. The DMP model describes the motion trajectory in a parameterized manner, and these parameters include the parameters of the central attraction potential function, the weight of the Gaussian kernel function, etc. The input start point, end point, and posture information will affect the values ​​of these parameters, thereby generating a motion trajectory that is suitable for the current task. Based on the input information and parameters, the DMP model generates an expected motion trajectory of the end effector of the robot arm, the robot arm joint, and the chassis position over time through a series of differential equation solving processes. These trajectories not only take into account the rotation or push-pull action, but also combine the coordinated control of the robot arm and the chassis to ensure that the two can work seamlessly when performing the door opening action. During the execution of the motion trajectory, the robot needs to monitor its current position, speed, acceleration and other information, as well as the contact status with the door handle in real time. If a deviation from the expected is detected, the system should be able to adjust the parameters in the DMP model in real time to update the motion trajectory to ensure that the robot can complete the task safely and accurately. In addition, through feedback control, the coordinated movement of the robot arm and chassis can be further optimized to improve the smoothness and stability of the entire door opening process.

[0079] In addition to the trajectory of turning the handle, the expected motion trajectory also includes the trajectory of pushing the door open. Figure 4 is a schematic diagram of a trajectory of a method for controlling a robot to open a door according to an optional embodiment of the present invention. Figure 4As shown, the left side is the unlocking trajectory, that is, the trajectory of twisting the handle, and the right side is the door pushing motion trajectory. A target position can be defined, where the target position refers to the position required for pushing the door, which depends on the size of the door and the distance required to open it. For the door pushing action, some basic DMP motion patterns can also be generated in advance through teaching learning. These patterns include linear motion along the door plane, as well as possible door handle grasping and releasing actions. The teaching data should contain various possible positions and states of the door so that the DMP model can be generalized to different scenarios. Then, by teaching the door pushing actions of multiple doors, the pose data of the robot end effector and chassis are collected. The teaching can include the complete process from grasping the door handle to pushing the door and releasing the door handle. Using the collected teaching data, the DMP model is trained through techniques such as GMM (Gaussian mixture model) and GMR (Gaussian mixture regression). GMM is used to model the distribution of teaching data, while GMR is used to generate new motion trajectories from GMM. According to the specific door and environmental conditions, the parameters of the DMP model, such as the parameters of the central attraction potential function and the weight of the Gaussian kernel function, are adjusted to generate a door-pushing trajectory that adapts to the current situation.

[0080] In summary, by inputting the posture information, the position information of the manipulator and the chassis into the trained target DMP model, the robot can generate the expected motion trajectory adapted to the specific handle type, and then guide the movement of the manipulator and the chassis to achieve autonomous and intelligent door opening operation. This process not only reflects the flexibility and generalization ability of the DMP model, but also reflects the adaptability and autonomy of the robot in complex environments.

[0081] As an optional embodiment, an action sequence is generated based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model, including: defining a cost function, wherein the cost function is used to evaluate the performance of the predicted action sequence; determining the constraints corresponding to the robot arm and the chassis based on the performance parameters corresponding to the robot arm and the chassis; integrating the dynamic models corresponding to the robot arm and the chassis into the model-based predictive control model to obtain a target predictive control model; based on the predicted motion trajectory, the cost function, and the constraints corresponding to the robot arm and the chassis, the target predictive control model is used to generate an action sequence.

[0082] Optionally, Figure 5 It is an MPC framework diagram provided according to an optional embodiment of the present invention. Based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control (MPC) model, the process of generating an action sequence generally involves the following steps: Defining the cost function: The cost function is the core of the MPC model, which quantifies the degree of deviation of the predicted action sequence from the task goal. For the robot door-pushing task, the cost function may include the following components:

[0083] a. Tracking error: This is the deviation between the actual position of the robot end effector and the target position of the predicted motion trajectory, and the deviation between the actual position of the chassis and the predicted position.

[0084] b. Controlling input costs: This may involve penalizing changes in robot arm joint angles, velocities, and accelerations, as well as penalizing changes in chassis linear and angular velocities to minimize energy consumption and avoid jerky movements.

[0085] c. Collision risk: If there are obstacles in the environment, the cost function may include predictions and penalties for possible collisions.

[0086] d. Time cost: The time required to complete a task to encourage fast and stable operations.

[0087] Specifically, the cost function can be:

[0088]

[0089] Among them, X N is the system state sequence, denoted by X N ={x k ,x k+1 ,…,x k+N};U N is the system input sequence, denoted as U N = {u k ,u k+1 ,…,u k+N}; L(x i ,u i ) is the cost function, expressed as Indicates the system input limit to avoid high input values ​​of the system, (P i -P d ) T Q a (P i -P d )+(R i -R d ) T Q r (R i -R d ) represents the position error and attitude error between the end effector of the robot and the desired posture.

[0090] Determine the constraints: The constraints are set based on the performance parameters of the robot arm and chassis, including:

[0091] a. Robot arm joint angle range: Make sure the joints do not exceed their physical limits.

[0092] b. Robot arm joint speed and acceleration limits: avoid too fast or too violent movement to protect the structural integrity of the robot arm and extend its life.

[0093] c. Chassis speed and acceleration limits: Ensure that the chassis moves smoothly and does not become unstable or out of control due to excessively fast or violent movements.

[0094] d. Torque and force constraints: Prevent the robot arm and chassis from being subjected to excessive torque or force during operation to protect the robot structure and motor.

[0095] Specifically, the constraints may include state machine constraints and control set constraints. The state set constraints include the feasible space of the chassis, the position limits of the robot joints, etc. The control set constraints include the maximum and minimum speed constraints of the chassis, and the maximum speed of the robot joints. Since the mobile chassis can reach any position, the state constraint set of the mobile robot is equivalent to the joint state constraint set X of the robot. mm ,The control input constraint set of the mobile robot is the linear velocity and angular velocity of the chassis and the joint angular velocity of the robot, denoted as U mm .

[0096] Integrate the dynamic model into the MPC model: The dynamic model of the manipulator and chassis is integrated into the MPC framework, which means that when solving the optimization problem, MPC will take into account the dynamic characteristics of the manipulator and chassis, including inertia, friction, gravity and other factors. Then generate the action sequence, using the MPC model, based on the expected motion trajectory, cost function and constraints, to generate the action sequence that meets the task requirements. This process involves the following steps:

[0097] a.MPC optimization problem setting: Set the optimization problem so that the cost function is minimized while satisfying all constraints.

[0098] b. Dynamic optimization solution: At each sampling time, the MPC solver will solve the optimal action sequence based on the current state and the future predicted state.

[0099] c. Action sequence execution: The solved action sequence is input as a control signal to the robot arm and chassis to execute the door-pushing action.

[0100] d. Real-time feedback and adjustment: During execution, the robot monitors the actual status of the robotic arm and chassis in real time, and feeds these statuses back to the MPC model to adjust the trajectory to ensure the real-time and robustness of the action sequence.

[0101] Specifically, CasADi can be used. In the MPC optimization problem of the mobile robotic arm system, the variable definition function of CasADi can be used to define the system state variables, control input variables, and prediction time domain. According to the optimization goal defined previously, the expression construction function of CasADi can be used to construct the objective function, and the IPOPT solver of CasADi can be called to solve the optimization problem, obtain the control input of the system, and control the mobile robotic arm to complete the door pushing operation.

[0102] Through the above steps, the MPC model can generate the optimal action sequence and guide the robot to complete the door-pushing task by considering the dynamic characteristics of the robot arm and chassis and environmental constraints. This method can not only handle complex dynamic problems, but also adapt to environmental changes and improve the accuracy and efficiency of operations.

[0103] Figure 6 is a system schematic diagram of a method for controlling a robot to open a door according to an optional embodiment of the present invention, and provides an optional system for controlling a robot to open a door, such as Figure 6 As shown, it includes a visual recognition and positioning module, a robotic arm unlocking module and a chassis and robotic arm coordination control module, wherein the visual recognition and positioning module can identify the door handle and obtain the door handle posture information, and then the robotic arm unlocking module can generate the robotic arm door handle twisting trajectory based on the DMP (dynamic motion primitive) method to achieve unlocking; then the chassis y6u robotic arm coordination control module is used to achieve the coordinated control of the robotic arm and the chassis to push the door based on the model predictive control (MPC) method.

[0104] Among them, by using visual sensors to identify door handles and obtain their posture information, the target can be accurately located in a complex and changeable actual environment, providing accurate starting conditions for subsequent robot arm operations. Based on the DMP method, the motion trajectory of the robot arm can be dynamically generated and optimized according to the real-time door handle posture information, so that it can complete the action of twisting the door handle more flexibly and accurately, overcoming the difficulty that the pre-programmed trajectory in the traditional robot arm door opening solution cannot adapt to various changes in actual operations. The model predictive control (MPC) method is used to realize the coordinated control of the robot arm and the chassis to push the door, which can fully consider the dynamic characteristics of the robot arm and the chassis and the interaction between them, and realize the coordinated movement of the two through rolling prediction and optimization. The problem of precise synchronization and coordinated control between the chassis and the robot arm when the mobile robot arm opens the door is solved.

[0105] It should be noted that, for the above-mentioned method embodiments, for the sake of simplicity, they are all described as a series of action combinations, but those skilled in the art should know that the present invention is not limited by the described action sequence, because according to the present invention, certain steps can be performed in other sequences or simultaneously. Secondly, those skilled in the art should also know that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily required by the present invention.

[0106] Through the description of the above implementation methods, those skilled in the art can clearly understand that the method for controlling the robot to open the door according to the above embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes a number of instructions for enabling a terminal device (which can be a mobile phone, computer, server, or network device, etc.) to execute the methods described in each embodiment of the present invention.

[0107] According to an embodiment of the present invention, a device for controlling a robot to open a door is also provided for implementing the above method for controlling a robot to open a door. Figure 7 is a structural block diagram of a device for controlling a robot to open a door according to an embodiment of the present invention, such as Figure 7 As shown, the device for controlling the robot to open the door includes: a first acquisition module 702, an input module 704, a second acquisition module 706, an establishment module 708, a generation module 710 and a control module 712. The device for controlling the robot to open the door is described below.

[0108] The first acquisition module 702 is used to acquire the position information corresponding to the target handle and the position information corresponding to the robot arm and the chassis of the target robot.

[0109] The input module 704 is connected to the first acquisition module 702 and is used to input the posture information and the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory.

[0110] The second acquisition module 706 is connected to the input module 704 and is used to acquire state information corresponding to the robot arm and the chassis, wherein the state information includes movement speed.

[0111] The establishing module 708 is connected to the second acquiring module 706 and is used to establish the dynamic models corresponding to the robot arm and the chassis respectively based on the state information corresponding to the robot arm and the chassis respectively.

[0112] The generation module 710 is connected to the establishment module 708 and is used to generate an action sequence based on the expected motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model.

[0113] The control module 712 is connected to the generation module 710 and is used to control the robot arm and the chassis to perform door opening movement based on the action sequence.

[0114] It should be noted that the first acquisition module 702, input module 704, second acquisition module 706, establishment module 708, generation module 710 and control module 712 correspond to steps S202 to S212 in the embodiment, and the examples and application scenarios implemented by the multiple modules and the corresponding steps are the same, but are not limited to the contents disclosed in the above embodiment. It should be noted that the above modules, as part of the device, can be run in the computer terminal 10 provided in the embodiment.

[0115] An embodiment of the present invention may provide a computer device. Optionally, in this embodiment, the computer device may be located in at least one network device among multiple network devices of a computer network. The computer device includes a memory and a processor.

[0116] Among them, the memory can be used to store software programs and modules, such as the program instructions / modules corresponding to the method and device for controlling the robot to open the door in the embodiment of the present invention. The processor executes various functional applications and data processing by running the software programs and modules stored in the memory, that is, realizing the above-mentioned method for controlling the robot to open the door. The memory may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory may further include a memory remotely arranged relative to the processor, and these remote memories may be connected to the computer terminal via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0117] The processor can call the information and application programs stored in the memory through the transmission device to execute the following steps: obtain the posture information corresponding to the target handle and the position information corresponding to the mechanical arm and the chassis of the target robot; input the posture information and the position information corresponding to the mechanical arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory; obtain the state information corresponding to the mechanical arm and the chassis, wherein the state information includes the motion speed; based on the state information corresponding to the mechanical arm and the chassis, establish the dynamic models corresponding to the mechanical arm and the chassis; generate an action sequence based on the expected motion trajectory, the dynamic models corresponding to the mechanical arm and the chassis, and the model-based predictive control model; based on the action sequence, control the mechanical arm and the chassis to perform the door opening movement.

[0118] Optionally, the processor may also execute program code for the following steps: obtaining posture information corresponding to the target handle, including: obtaining image data of the target handle; inputting the image data into a preset target recognition model to obtain edge information of the target handle; determining a depth image corresponding to the image data; determining a depth value corresponding to the target handle based on the depth image; and determining the posture information of the target handle based on the edge information and the depth value.

[0119] Optionally, the processor may also execute program codes of the following steps: before inputting the posture information, the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory, it includes: obtaining multiple teaching trajectories corresponding to each of the multiple handle types; using the multiple teaching trajectories corresponding to each of the multiple handle types to train the preset original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0120] Optionally, the processor may also execute program code for the following steps: using multiple teaching trajectories corresponding to each of the multiple handle types to train a preset original dynamic motion primitive model to obtain a target dynamic motion primitive model, including: calculating the smoothness of each of the multiple teaching trajectories corresponding to each of the multiple handle types; selecting a teaching trajectory whose smoothness meets a preset condition among the multiple teaching trajectories corresponding to each of the multiple handle types as a target teaching trajectory to obtain a target teaching trajectory corresponding to each of the multiple handle types; training the original dynamic motion primitive model based on the target teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model.

[0121] Optionally, the processor may also execute the program code of the following steps: training a preset original dynamic motion primitive model using the teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: extracting feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types based on a Gaussian mixture model, wherein the feature points represent the key position of the handle of the corresponding handle type during the screwing process; adjusting the parameters in the original dynamic motion primitive model based on the Gaussian mixture regression model and the feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model.

[0122] Optionally, the processor may also execute the program code of the following steps: inputting the posture information, the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain an expected motion trajectory, including: determining the target handle type of the target handle based on the posture information; determining the starting point information and end point information of the handle rotation corresponding to the target handle type; inputting the starting point information, end point information, posture information, and the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain an expected motion trajectory.

[0123] Optionally, the processor may also execute program code for the following steps: generating an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and chassis, and a model-based predictive control model, including: defining a cost function, wherein the cost function is used to evaluate the performance of the predicted action sequence; determining the constraints corresponding to the robot arm and chassis based on the performance parameters corresponding to the robot arm and chassis; integrating the dynamic models corresponding to the robot arm and chassis into the model-based predictive control model to obtain a target predictive control model; generating an action sequence using the target predictive control model based on the predicted motion trajectory, the cost function, and the constraints corresponding to the robot arm and chassis.

[0124] By adopting an embodiment of the present invention, a scheme for a method of controlling a robot to open a door is provided, by obtaining posture information corresponding to a target handle and position information corresponding to a mechanical arm and a chassis of a target robot; inputting the posture information and the position information corresponding to the mechanical arm and the chassis into a target dynamic motion primitive model to obtain an expected motion trajectory; obtaining state information corresponding to the mechanical arm and the chassis, wherein the state information includes a motion speed; establishing a dynamic model corresponding to the mechanical arm and the chassis based on the state information corresponding to the mechanical arm and the chassis; generating an action sequence based on the expected motion trajectory, the dynamic model corresponding to the mechanical arm and the chassis, and a predictive control model based on the model; and controlling the mechanical arm and the chassis to perform a door opening motion based on the action sequence, thereby achieving the purpose of generating an action sequence based on the expected motion trajectory to control the mechanical arm and the chassis, thereby realizing the technical effect of improving the flexibility of the robot and reducing the probability of accidents, thereby solving the technical problem that the mechanical arm and the chassis of the current robot have poor coordination, may not be able to accurately grasp the handle or complete the door opening motion according to the fixed trajectory motion, and is prone to accidents.

[0125] A person of ordinary skill in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructing the hardware related to the terminal device through a program, and the program can be stored in a non-volatile storage medium, and the storage medium may include: a flash drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, etc.

[0126] The embodiment of the present invention further provides a non-volatile storage medium. Optionally, in this embodiment, the non-volatile storage medium can be used to store the program code executed by the method for controlling the robot to open the door provided in the above embodiment.

[0127] Optionally, in this embodiment, the non-volatile storage medium may be located in any computer terminal in a computer terminal group in a computer network, or in any mobile terminal in a mobile terminal group.

[0128] Optionally, in this embodiment, the non-volatile storage medium is configured to store program codes for executing the following steps: obtaining posture information corresponding to the target handle and position information corresponding to the robotic arm and chassis in the target robot; inputting the posture information and the position information corresponding to the robotic arm and chassis into the target dynamic motion primitive model to obtain an expected motion trajectory; obtaining state information corresponding to the robotic arm and chassis, wherein the state information includes the motion speed; establishing a dynamic model corresponding to the robotic arm and chassis based on the state information corresponding to the robotic arm and chassis; generating an action sequence based on the expected motion trajectory, the dynamic models corresponding to the robotic arm and chassis, and a model-based predictive control model; and controlling the robotic arm and chassis to perform a door opening movement based on the action sequence.

[0129] Optionally, in this embodiment, the non-volatile storage medium is configured to store program code for executing the following steps: obtaining posture information corresponding to the target handle, including: obtaining image data of the target handle; inputting the image data into a preset target recognition model to obtain edge information of the target handle; determining a depth image corresponding to the image data; determining a depth value corresponding to the target handle based on the depth image; and determining the posture information of the target handle based on the edge information and the depth value.

[0130] Optionally, in this embodiment, the non-volatile storage medium is configured to store program codes for executing the following steps: before inputting the posture information, the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory, including: obtaining multiple teaching trajectories corresponding to each of the multiple handle types; using the multiple teaching trajectories corresponding to each of the multiple handle types to train the preset original dynamic motion primitive model to obtain the target dynamic motion primitive model.

[0131] Optionally, in this embodiment, the non-volatile storage medium is configured to store program code for executing the following steps: using multiple teaching trajectories corresponding to each of the multiple handle types to train a preset original dynamic motion primitive model to obtain a target dynamic motion primitive model, including: calculating the smoothness of each of the multiple teaching trajectories corresponding to each of the multiple handle types; selecting a teaching trajectory whose smoothness meets a preset condition among the multiple teaching trajectories corresponding to each of the multiple handle types as a target teaching trajectory to obtain the target teaching trajectory corresponding to each of the multiple handle types; based on the target teaching trajectories corresponding to each of the multiple handle types, training the original dynamic motion primitive model to obtain a target dynamic motion primitive model.

[0132] Optionally, in this embodiment, the non-volatile storage medium is configured to store program code for executing the following steps: training a preset original dynamic motion primitive model using teaching trajectories corresponding to each of the multiple handle types to obtain a target dynamic motion primitive model, including: based on a Gaussian mixture model, extracting feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, wherein the feature points represent the key position of the handle of the corresponding handle type during the screwing process; based on the Gaussian mixture regression model and the feature points corresponding to the teaching trajectories corresponding to each of the multiple handle types, adjusting the parameters in the original dynamic motion primitive model to obtain a target dynamic motion primitive model.

[0133] Optionally, in this embodiment, the non-volatile storage medium is configured to store program code for executing the following steps: inputting the posture information, the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain an expected motion trajectory, including: determining the target handle type of the target handle based on the posture information; determining the starting point information and end point information of the handle rotation corresponding to the target handle type; inputting the starting point information, end point information, posture information, and the position information corresponding to the robotic arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory.

[0134] Optionally, in this embodiment, the non-volatile storage medium is configured to store program code for executing the following steps: generating an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model, including: defining a cost function, wherein the cost function is used to evaluate the performance of the predicted action sequence; determining the constraints corresponding to the robot arm and the chassis based on the performance parameters corresponding to the robot arm and the chassis; integrating the dynamic models corresponding to the robot arm and the chassis into the model-based predictive control model to obtain a target predictive control model; generating an action sequence using the target predictive control model based on the predicted motion trajectory, the cost function, and the constraints corresponding to the robot arm and the chassis.

[0135] An embodiment of the present invention also provides a computer program product, including a computer program. Optionally, in this embodiment, when the computer program is executed by a processor, it can achieve: obtaining the posture information corresponding to the target handle and the position information corresponding to the robotic arm and chassis in the target robot; inputting the posture information and the position information corresponding to the robotic arm and chassis into the target dynamic motion primitive model to obtain an expected motion trajectory; obtaining the state information corresponding to the robotic arm and chassis, wherein the state information includes the motion speed; based on the state information corresponding to the robotic arm and chassis, establishing the dynamic models corresponding to the robotic arm and chassis; generating an action sequence based on the expected motion trajectory, the dynamic models corresponding to the robotic arm and chassis, and a model-based predictive control model; based on the action sequence, controlling the robotic arm and chassis to perform a door opening movement.

[0136] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0137] In the above embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0138] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only schematic. For example, the division of the units can be a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0139] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0140] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0141] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a non-volatile storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, server or network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes.

[0142] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for controlling a robot to open a door, characterized in that: include: Obtain the position information corresponding to the target handle and the position information corresponding to the manipulator arm and chassis in the target robot; Inputting the posture information, the position information corresponding to the robot arm and the chassis into a target dynamic motion primitive model to obtain an estimated motion trajectory; Acquire state information corresponding to the robot arm and the chassis respectively, wherein the state information includes movement speed; Based on the state information corresponding to the mechanical arm and the chassis, respectively, a dynamic model corresponding to the mechanical arm and the chassis is established; Generate an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and a model-based predictive control model; Based on the action sequence, the robotic arm and the chassis are controlled to perform door opening movement.

2. The method according to claim 1, characterized in that The obtaining of the position information corresponding to the target handle includes: Acquire image data of the target handle; Inputting the image data into a preset target recognition model to obtain edge information of the target handle; Determining a depth image corresponding to the image data; Determining a depth value corresponding to the target handle according to the depth image; Based on the edge information and the depth value, the position and posture information of the target handle is determined.

3. The method according to claim 1, characterized in that Before inputting the posture information, the position information corresponding to the robot arm and the chassis into the target dynamic motion primitive model to obtain the expected motion trajectory, the method includes: Obtain multiple teaching trajectories corresponding to multiple handle types; The preset original dynamic motion primitive model is trained using a plurality of teaching trajectories corresponding to the plurality of handle types to obtain the target dynamic motion primitive model.

4. The method according to claim 3, characterized in that: The method of training a preset original dynamic motion primitive model using a plurality of teaching trajectories corresponding to each of the plurality of handle types to obtain the target dynamic motion primitive model includes: Calculating the smoothness of each of the plurality of teaching trajectories corresponding to each of the plurality of handle types; Selecting a teaching trajectory whose smoothness meets a preset condition from among the multiple teaching trajectories corresponding to the multiple handle types as a target teaching trajectory, and obtaining the target teaching trajectory corresponding to the multiple handle types; Based on the target teaching trajectories corresponding to the multiple handle types, the original dynamic motion primitive model is trained to obtain the target dynamic motion primitive model.

5. The method according to claim 3, characterized in that: The method of training a preset original dynamic motion primitive model using the teaching trajectories corresponding to the plurality of handle types to obtain the target dynamic motion primitive model includes: Based on the Gaussian mixture model, feature points corresponding to the teaching trajectories corresponding to the multiple handle types are extracted, wherein the feature points represent key positions of the handles of the corresponding handle types during the screwing process; Based on the Gaussian mixture regression model and the feature points corresponding to the teaching trajectories corresponding to the multiple handle types, the parameters in the original dynamic motion primitive model are adjusted to obtain the target dynamic motion primitive model.

6. The method according to claim 1, characterized in that The step of inputting the posture information, the position information corresponding to the robot arm and the chassis into a target dynamic motion primitive model to obtain an estimated motion trajectory includes: Determining a target handle type of the target handle according to the position information; Determine the starting point information and the ending point information of the handle rotation corresponding to the target handle type; The starting point information, the end point information, the posture information, and the position information corresponding to the robot arm and the chassis are input into the target dynamic motion primitive model to obtain the expected motion trajectory.

7. The method according to claim 1, characterized in that The generating of an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and the model-based predictive control model includes: defining a cost function, wherein the cost function is used to evaluate the performance of the predicted action sequence; Determining constraint conditions corresponding to the mechanical arm and the chassis respectively based on performance parameters corresponding to the mechanical arm and the chassis respectively; Integrating the dynamic models corresponding to the robot arm and the chassis into the model-based predictive control model to obtain a target predictive control model; Based on the predicted motion trajectory, the cost function, and the constraints corresponding to the robot arm and the chassis, the target predictive control model is used to generate the action sequence.

8. A device for controlling a robot to open a door, characterized in that: include: The first acquisition module is used to acquire the position information corresponding to the target handle and the position information corresponding to the mechanical arm and the chassis of the target robot; An input module, used for inputting the posture information, the position information corresponding to the robot arm and the chassis into a target dynamic motion primitive model to obtain an estimated motion trajectory; A second acquisition module is used to acquire state information corresponding to the robot arm and the chassis, wherein the state information includes a movement speed; An establishing module, used for establishing a dynamic model corresponding to the mechanical arm and the chassis respectively based on the state information corresponding to the mechanical arm and the chassis respectively; A generation module, configured to generate an action sequence based on the predicted motion trajectory, the dynamic models corresponding to the robot arm and the chassis, and a model-based predictive control model; A control module is used to control the mechanical arm and the chassis to perform door opening motion based on the action sequence.

9. A non-volatile storage medium, characterized in that: The non-volatile storage medium includes a stored program, wherein when the program is running, the device where the non-volatile storage medium is located is controlled to execute the method for controlling a robot to open a door as described in any one of claims 1 to 7.

10. A computer device, characterized in that: include: Memory and processor, The memory stores a computer program; The processor is used to execute the computer program stored in the memory, and when the computer program is run, the processor executes the method for controlling the robot to open the door as described in any one of claims 1 to 7.

11. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the method for controlling a robot to open a door as described in any one of claims 1 to 7 is implemented.

Images