Robot motion control method, robot, storage medium and program product
Through the combination of the admittance control model with fewer dimensions and high-dimensional motion control model, the problems of large amount of computing and low efficiency in multi-dimensional motion control are solved, and the precise control of robot motion is realized, functional abnormality compensation is avoided, and multiple satisfactory Needs for joint exercise rehabilitation training.
Patent Information
- Application Number
- CN202410943366.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-15
- Publication Date
- 2024-11-08
AI Technical Summary
The existing multi-dimensional admittance control model has a large amount of calculation in robot motion control, which occupies resources and affects efficiency, making it difficult to achieve accurate and timely motion control, especially in the human multi-joint motion rehabilitation training.
The admission control model with fewer dimensions combined with the high-dimensional motion control model is used to obtain the motion quantity characteristics through the admission control model of the first coordinate system, and the target motion quantity is mapped according to the preset trajectory motion method in the second coordinate system, so as to control the robot to achieve the The target position allows accurate control of high-dimensional motion.
It effectively reduces the amount of calculation, improves the accuracy and efficiency of robot motion control, avoids functional abnormalities, and meets the needs of multi-joint exercise rehabilitation training.
Smart Images

Figure CN118915436A_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates to the technical field of robot motion control, and in particular to a robot motion control method, a robot, a storage medium, and a program product. Background Art
[0002] There are two problems that may affect the rehabilitation effect during the human motor function rehabilitation training: (1) low motor function participation and low repeatability; (2) abnormal functional compensation.
[0003] In sports rehabilitation training, in order to improve the rehabilitation effect, sports participation and high-repetition training are emphasized. Patients with motor dysfunction need external assistance due to their own insufficient motor function, but the assistance intensity often cannot be adjusted in real time, resulting in high assistance intensity, but low patient sports participation, and weak motor function cannot support high-repetition, high-intensity training, resulting in reduced rehabilitation training effect. To address this type of problem, rehabilitation robots use more precise mechanical interaction to achieve real-time adjustment of assistance intensity to match the assistance intensity with the patient's sports participation and high-repetition training requirements. Among them, the most commonly used mechanical control method for rehabilitation robots is admittance control. Admittance control is to convert the external input force into the robot's motion control target, and maintain appropriate mechanical interaction with the outside world through the robot's motion ability to achieve real-time adjustment of the assistance intensity required in rehabilitation training. Therefore, admittance control has become an important force control method for rehabilitation robots.
[0004] On the other hand, during rehabilitation training, patients may have better functioning parts that help weaker function parts achieve the expected movement, resulting in the weaker function parts failing to achieve the expected training goals. This type of motor function compensation can be defined as abnormal functional compensation, especially in training involving multiple joint movements. According to the movement mode of human joints, rehabilitation robots can be divided into end-traction type and articulated type. The articulated type generally refers to human powered exoskeleton, which can directly drive the patient's specific joints to perform passive / active movement through single / multiple motors; the end-traction type refers to the rehabilitation robot contacting the human body through a certain point in the motion mechanism, and the robot drives the end of the human joint to perform passive / active movement through the end. In view of the movement dysfunction of human joints, end-traction rehabilitation robots must realize the movement of the joint end to drive the joint movement through a specific trajectory. Therefore, in order to achieve the expected movement of each joint and avoid abnormal functional compensation, the movement trajectory is more important for end-traction robots.
[0005] In order to meet the requirements of multi-joint motion rehabilitation training for the human body, the rehabilitation robot also has multi-degree-of-freedom motion, so the motion trajectory also has multi-degree-of-freedom motion characteristics, that is, the trajectory moves in different directions in space, so the motion trajectory can be further combined with rigid body motion, that is, rotational motion is added, then the multi-dimensional motion trajectory becomes a multi-dimensional motion including rotation, which is described here as multiple motion dimensions. The principle of the admittance control model is to simulate the changes of the virtual dynamic system according to external input. However, the current method of using the multi-dimensional admittance control model to achieve motion control goals has a large amount of calculation, occupies computing resources and affects efficiency, which affects the accuracy and timeliness of robot control. Summary of the invention
[0006] In view of the above-mentioned shortcomings of the prior art, the purpose of the present disclosure is to provide a robot motion control method, a robot, a storage medium and a program product to solve the problems in the related art.
[0007] A first aspect of the present disclosure provides a robot motion control method, comprising: in a current control cycle, obtaining an external force on the robot; determining a motion quantity characteristic of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system; the motion quantity characteristic includes a first target position and / or a first target angle; through a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, using the motion quantity characteristic as a control quantity, mapping the control quantity into a target motion quantity that moves in a preset trajectory in the second coordinate system, determining a target posture of the current control cycle achieved by the target motion quantity, the target posture including a second target position or a second target position and a second target angle; and controlling the robot movement to achieve the target posture.
[0008] In an embodiment of the first aspect, the admittance control model includes: a determined relationship between a comprehensive result of one or more physical parameters of the robot's inertia, damping, elasticity, and nonlinear force and the residual force after the external force overcomes the preset interaction force of the robot; the inertia is related to the robot's first target acceleration; the damping is related to the speed change of the robot's first current speed relative to the preset speed; the elasticity is related to the position change of the robot's first current position relative to the preset position; the motion characteristic includes a first target position, and the motion characteristic of the robot in the first coordinate system is determined based on the external force through the admittance control model related to the first coordinate system, including: determining the first target acceleration according to the first current speed and the first current position through the admittance control model; determining the first target speed based on the first current speed, the first target acceleration and the current control cycle duration; determining the first target position based on the first current position, the first target speed and the current control cycle duration.
[0009] In an embodiment of the first aspect, the motion control model includes: a calculation relationship in which the starting posture of the preset trajectory motion mode reaches the target posture under the action of the motion control model parameters; the starting posture includes a starting position and a starting angle; and the motion control model parameters are determined by the preset trajectory motion mode.
[0010] In an embodiment of the first aspect, the target posture includes a second target position; the motion control model parameters include: a rotation parameter, a deflection angle from the starting position of the starting posture to the second target position of the target posture when the robot moves in a preset trajectory motion manner; the motion control model related to the second coordinate system with a higher dimension than the first coordinate system, takes the motion quantity characteristic as the control quantity, maps the control quantity to the target motion quantity in the second coordinate system according to the preset trajectory motion manner, and determines the target posture of the current control cycle achieved by the target motion quantity, including: a rotation parameter based on the starting position and the deflection angle obtained from the first target position, to obtain the second target position of the target posture.
[0011] In an embodiment of the first aspect, the target posture also includes a target angle; the motion control model parameters also include: a rotation parameter, which is related to the proportional relationship between the deflection angle from the starting posture to the target posture of the preset trajectory motion mode and the total deflection angle from the starting posture to the end posture when the robot rotates in the preset trajectory motion mode; the motion control model related to the second coordinate system with a higher dimension than the first coordinate system uses the motion quantity characteristic as the control quantity, maps the control quantity to the target motion quantity in the second coordinate system according to the preset trajectory motion mode, and determines the target posture of the current control cycle achieved by the target motion quantity, and also includes: obtaining the total rotation angle between the starting angle at the starting position and the end angle at the end position, and obtaining the second target angle of the target posture based on the partial angle of the total rotation angle under the proportional relationship.
[0012] In an embodiment of the first aspect, the rotation parameters include: at least one rotation matrix constructed by the angle between a first position vector pointing to a starting position on the arc / sphere and a second position vector pointing to a second target position on at least one arc / sphere passing through a preset trajectory motion mode.
[0013] In an embodiment of the first aspect, the preset trajectory motion mode includes circular arc / spherical trajectory motion; the motion control model related to a second coordinate system with a higher dimension than the first coordinate system uses the motion quantity characteristic as the control quantity, maps the control quantity into a target motion quantity in the second coordinate system according to the preset trajectory motion mode, and determines the target posture of the current control cycle achieved by the target motion quantity, including: the motion control model obtains a first position vector pointing to the starting position from the center of the circle / center of the sphere based on the coordinates of the center of the circle / center of the sphere and the starting position, obtains a second position vector pointing to the second target position from the center of the circle / center of the sphere based on the first position vector and the rotation parameter, and obtains the second target position based on the coordinates of the center of the circle / center of the sphere and the second position vector.
[0014] In an embodiment of the first aspect, the preset trajectory motion mode includes circular / spherical motion; the motion control model related to the second coordinate system with a higher dimension than the first coordinate system takes the motion quantity characteristic as the control quantity, maps the control quantity into the target motion quantity according to the preset trajectory motion mode in the second coordinate system, and determines the target posture of the current control cycle achieved by the target motion quantity, including: the change angle obtained based on the angle between the starting angle of the starting posture and the end angle of the end posture under the proportional relationship, and the target angle is obtained based on the sum of the change angle and the starting angle.
[0015] In an embodiment of the first aspect, the first coordinate system is a one-dimensional or multi-dimensional coordinate system including at least one movement dimension and / or at least one rotation dimension, and the second coordinate system is a multi-dimensional coordinate system including multiple movement dimensions or multiple movement dimensions and at least one rotation dimension.
[0016] The second aspect of the present disclosure provides a robot, comprising: a collection device for collecting external forces acting on the robot; a motion execution device for executing actions; a control device, comprising: a processor, a memory and a communication interface; the communication interface is communicatively coupled to the collection device and the motion execution device; the memory stores program instructions; the processor is used to run the program instructions to execute the robot motion control method as described in any one of the first aspects, generate control instructions and send them to the motion execution device through the communication interface, so that the motion execution device executes actions according to the control instructions to achieve the target posture.
[0017] A third aspect of the present disclosure provides a computer-readable storage medium storing program instructions, wherein when the program instructions are executed, the robot motion control method as described in any one of the first aspects is executed.
[0018] A fourth aspect of the present disclosure provides a computer program product, comprising: a computer program for executing the robot motion control method as described in any one of the first aspects.
[0019] As described above, the present disclosure provides a motion control method, a robot, a storage medium and a program product for a robot, wherein the method comprises: obtaining the external force on the robot in the current control cycle; determining the motion quantity characteristics of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system; the motion quantity characteristics include at least one of the following: a first target position and a first target angle; using a motion control model related to a second coordinate system with a dimension higher than the first coordinate system, using the motion quantity characteristics as the control quantity, mapping the control quantity to the target motion quantity in the second coordinate system according to a preset trajectory motion mode, determining the target posture of the current control cycle achieved by the target motion quantity; and controlling the robot to achieve the target posture. Thus, it is possible to control a high-dimensional motion control model through a low-dimensional admittance control model, and effectively reduce the amount of calculation while ensuring precise motion control. BRIEF DESCRIPTION OF THE DRAWINGS
[0020] Figure 1 A schematic diagram showing a flow chart of a robot motion control method in one embodiment of the present disclosure.
[0021] Figure 2 A schematic diagram showing the calculation principle of a two-dimensional motion control model based on the circular motion of a robot in one embodiment of the present disclosure is shown.
[0022] Figure 3 A schematic diagram of the circuit structure of a control device in an embodiment of the present disclosure is shown.
[0023] Figure 4 A schematic diagram showing the structure of a robot in one embodiment of the present disclosure.
[0024] Figure 5 A schematic diagram showing the structure of a control device in an embodiment of the present disclosure is shown.
[0025] Figure 6 A schematic diagram showing a module of a motion control device for a robot in an embodiment of the present disclosure. DETAILED DESCRIPTION
[0026] The following is an explanation of the embodiments of the present disclosure by specific examples, and those skilled in the art can easily understand other advantages and effects of the present disclosure from the information disclosed by the present disclosure. The present disclosure can also be implemented or applied through other different specific embodiments, and the details in the present disclosure can also be modified or changed in various ways according to different viewpoints and application modules without departing from the spirit of the present disclosure. It should be noted that the embodiments in the present disclosure and the features in the embodiments can be combined with each other without conflict.
[0027] The following is a detailed description of the embodiments of the present disclosure with reference to the accompanying drawings so that those skilled in the art can easily implement the present disclosure. The present disclosure can be embodied in many different forms and is not limited to the embodiments described herein.
[0028] In the representations of the present disclosure, the reference terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" and the like mean that the specific features, structures, materials or characteristics represented in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. Moreover, the specific features, structures, materials or characteristics represented may be combined in any one or a group of embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples represented in the present disclosure and the features of different embodiments or examples, unless they are mutually contradictory.
[0029] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the representation of the present disclosure, "a group" means two or more, unless otherwise clearly and specifically defined.
[0030] In order to clearly describe the present disclosure, components not related to the description are omitted, and the same reference numerals are given to the same or similar components throughout the specification.
[0031] Throughout the specification, when a device is said to be "connected" to another device, this includes not only the case of "direct connection" but also the case of "indirect connection" by placing other elements therebetween. In addition, when a device is said to "include" a certain component, unless otherwise stated, it does not exclude other components, but means that other components may be included.
[0032] Although the terms first, second, etc. are used to represent various elements in this article in some examples, these elements should not be limited by these terms. These terms are only used to distinguish one element from another element. For example, the first interface and the second interface, etc. are represented. Moreover, as used in this article, the singular forms "one", "one" and "the" are intended to also include plural forms, unless there is an opposite indication in the context. It should be further understood that the terms "comprising" and "including" indicate that there are the described features, steps, operations, elements, modules, projects, kinds, and / or groups, but do not exclude the existence, occurrence or addition of one or a group of other features, steps, operations, elements, modules, projects, kinds, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or mean any one or any combination. Therefore, "A, B or C" or "A, B and / or C" means "any of the following: A; B; C; A and B; A and C; B and C; A, B and C". Only when the combination of elements, functions, steps or operations is inherently mutually exclusive in some way, will there be an exception to this definition.
[0033] The technical terms used herein are only used to refer to specific embodiments and are not intended to limit the present disclosure. The singular form used herein also includes the plural form unless the sentence clearly indicates the contrary meaning. The meaning of "including" used in the specification is to specify specific characteristics, regions, integers, steps, operations, elements and / or components, and does not exclude the existence or addition of other characteristics, regions, integers, steps, operations, elements and / or components.
[0034] Although not defined differently, all terms, including technical and scientific terms used herein, have the same meaning as those generally understood by those skilled in the art to which the present disclosure belongs. Terms defined in commonly used dictionaries are additionally interpreted as having meanings consistent with relevant technical literature and the message of the present disclosure, and shall not be overly interpreted as ideal or very formal meanings unless defined.
[0035] At present, for the control of the robot's multi-dimensional motion, the admittance control model used is the corresponding multi-dimensional admittance control model. For example, a three-dimensional admittance control model is used to control three-dimensional motion, and a more-dimensional admittance control model is used to control more-dimensional motion. The amount of calculation will increase significantly, occupying computing resources and affecting efficiency, which will have an impact on the accuracy and timeliness of the robot control.
[0036] In view of this, a motion control method for a robot is provided in an embodiment of the present disclosure, which can realize the "few-dimensional" admittance control model with fewer dimensions to control the robot to perform "multi-dimensional" motion with more dimensions. The following is an illustration using a rehabilitation training robot as an example. The rehabilitation training robot can be used to assist people with physical movement disorders to actively or passively perform rehabilitation training movements to overcome obstacles. For example, the rehabilitation training robot may include an end effector that moves in a two-dimensional plane and can rotate in one dimension, and the end effector is provided to interact with the user's limbs (such as hands) so as to be driven by the user's force when the user's limbs move, or to provide a force to drive the user's limbs to move / rotate, etc. It should be particularly noted that the rehabilitation training robot is only an example of a robot, and other types of robots may also be used in other embodiments, all of which can apply the motion control method in the embodiment of the present disclosure, and are not limited to rehabilitation training robots.
[0037] The principle of motion control method is briefly explained. In order to meet the requirements of multi-joint motion rehabilitation training of the human body, the rehabilitation robot also has multi-degree-of-freedom motion, so the motion trajectory also has multi-degree-of-freedom motion characteristics, that is, the trajectory moves in different directions in space, so the motion trajectory can be further combined with rigid body motion, that is, rotational motion is added, and the multi-dimensional motion trajectory can be a multi-dimensional motion without / with rotation. The principle of the admittance control model is to simulate the changes of the virtual dynamic system according to the external input. The dynamic system can also have multiple degrees of freedom, that is, motion in "multiple motion dimensions".
[0038] It is worth noting that the multi-joint motion of the human body generally presents the characteristics of a certain trajectory at the end, that is, the end usually moves along a certain spatial curve in space, and its motion characteristics can be regarded as single-degree-of-freedom motion in rigid body motion, that is, motion in a "single motion dimension". Therefore, the characteristics of the motion of the end along the spatial curve can be used to design an admittance control model with a correspondingly smaller motion dimension (referred to as a "few-dimensional admittance control model") to control multi-dimensional motion with more motion dimensions.
[0039] To explain intuitively through metaphor, the few-dimensional admittance control model can be considered as a car that can only move forward or backward, and the car is driving on a rugged and winding road. Although for the car, it is a single-dimensional movement forward or backward, from the perspective of the entire motion system, by controlling the slope and curvature of the road, the car can rise, fall, turn, and tilt, that is, the multi-dimensional movement of the position / attitude (angle) of the car can be controlled in multiple motion dimensions. The road can be regarded as a multi-dimensional motion control model. That is, the cooperation of the few-dimensional admittance control model and the multi-dimensional motion control model can be used to realize the control of the robot's multi-dimensional motion.
[0040] The robot in the embodiment of the present disclosure may be a rehabilitation training robot, which may include an actuator that can actively / passively perform one-dimensional, two-dimensional, three-dimensional or more motion dimensions, and the actuator has an end effector that interacts with the trainee's force. The active refers to that the robot can actively move to drive the trainee to move, and the passive refers to that the robot performs passive movement due to the force applied by the trainee. The movement dimension may be movement along one or more coordinate axes, or rotation around one or more coordinate axes. It should be particularly noted that the robot may also be other types of robots, and is not limited to the rehabilitation training robot in the embodiment of the present disclosure.
[0041] like Figure 1 As shown, a flow chart of a motion control method in an embodiment of the present disclosure is shown.
[0042] exist Figure 1 In the embodiment of the present invention, the motion control method comprises:
[0043] Step S101: In the current control cycle, the external force applied to the robot is obtained.
[0044] In some embodiments, the robot may detect external forces applied thereto by, for example, force sensors.
[0045] In some embodiments, the robot may be controlled periodically, so the external force data may be collected and the corresponding control may be performed in each control cycle. The result of the control in the current control cycle, such as the target position, may be used as reference data for the next control cycle.
[0046] Step S102: determining the motion characteristics of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system.
[0047] The motion feature includes at least one of the following: a first target position and a first target angle.
[0048] In some embodiments, the admittance control model is a virtual model that simulates the actual situation of a controller that reaches the predicted first target position under the action of an external force based on an input. Therefore, the physical elements between the external force and the first target position, such as the effects of inertia, damping, elasticity and nonlinear forces, are considered in the admittance control model. Specifically, the admittance control model includes a definite relationship between the comprehensive result of one or more physical parameters of the robot's inertia, damping, elasticity and nonlinear force and the residual force after the external force overcomes the preset interaction force of the robot. Among them, the inertia is related to the first target acceleration of the robot; the damping is related to the speed change of the first current speed of the robot relative to the preset speed; the elasticity is related to the position change of the first current position of the robot relative to the preset position.
[0049] The one-dimensional admittance control model and the motion characteristic as the first target position are used as an example for explanation. For example, a formula description of the one-dimensional admittance control model is given:
[0050]
[0051] Among them, M represents the actual external input force, M0 represents the target interaction force, which is the interaction force that the robot needs to have when no external force is applied, and the target interaction force needs to be overcome when external force is applied.
[0052] m represents the virtual quality parameter;
[0053] d represents the virtual damping parameter;
[0054] k represents the virtual elastic coefficient;
[0055] p represents the current acceleration, velocity, and position of the robot respectively;
[0056] p0 represents the preset speed and preset position of the robot;
[0057] f represents nonlinear forces such as virtual friction.
[0058] Thus, according to the admittance control model, the first target acceleration can be first determined according to the first current speed and the first current position through the admittance control model. Secondly, the first target speed is determined based on the first current speed, the first target acceleration and the current control cycle duration. Further, the first target position is determined based on the first current position, the first target speed and the current control cycle duration.
[0059] Taking formula (1) as an example, the robot motion target is calculated in the nth control cycle with a duration of Δt. When the actual external input force is M n When the target acceleration reached by the external input force in the current control cycle is calculated, the admittance control model is for
[0060]
[0061] Therefore, calculate the target speed achieved in the current control cycle for
[0062]
[0063] Calculate the target position p achieved in the current control cycle n+1 for
[0064]
[0065] As an example, p n+1 It can be used as the control quantity output of a one-dimensional admittance model for motion control of motion control models with higher motion dimensions.
[0066] The one-dimensional movement or rotation involved in the one-dimensional admittance control model is like the forward or backward movement of the "car" mentioned in the previous metaphor. Furthermore, the amount of the forward or backward movement of the "car" is used as the control amount, which is associated with the motion control model of a higher motion dimension. The robot motion model is used as the "highway" for realizing the movement or rotation of the "car", thereby achieving the result that the high-dimensional motion model can be controlled by the low-dimensional admittance control model.
[0067] Step S103: Through a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, the motion quantity characteristic is used as the control quantity, the control quantity is mapped into a target motion quantity according to a preset trajectory motion mode in the second coordinate system, and the target posture of the current control cycle achieved by the target motion quantity is determined.
[0068] In some embodiments, the first coordinate system is a one-dimensional or multi-dimensional coordinate system including at least one moving dimension and / or at least one rotational dimension. For example, the coordinate axis of only one moving dimension exemplified in formula (1). Alternatively, a coordinate system of more dimensions may also be used. The second coordinate system is a multi-dimensional coordinate system including multiple moving dimensions, or multiple moving dimensions and at least one rotational dimension. For example, a coordinate system of a two-dimensional mixture (referring to a mixture of movement and rotation) having both moving motion and rotation along an X-axis. For another example, a plane coordinate system that moves in a plane coordinate system including X and Y axes. For another example, a plane coordinate system that moves in a plane coordinate system including X and Y axes, and a three-dimensional mixed multi-dimensional coordinate system that can rotate around X and Y axes. For another example, a four-, five-, or six-dimensional coordinate system that moves in a spatial coordinate system including X, Y, and Z axes, and can rotate around one or more of the X, Y, and Z axes.
[0069] The motion control model implements the control quantity based on the output of the admittance control model as input, and outputs the target posture that the robot needs to reach accordingly. In some embodiments, the target posture may include a second target position, for example, only the second target position to which the robot needs to move can be calculated, that is, the rotation dimension of the robot is not considered. In other embodiments, considering the rotation dimension of the robot, the target posture may include the second target position and the second target angle of the robot (that is, the rotation angle of the robot around at least one coordinate axis in the second coordinate system). In some embodiments, the motion control model includes: the calculation relationship of the starting posture of the preset trajectory motion mode to reach the target posture under the action of the motion control model parameters; the starting posture includes a starting position and a starting angle. The motion control model parameters are determined by the preset trajectory motion mode.
[0070] In some embodiments, taking the target posture including a second target position as an example, the motion control model parameters include: a rotation parameter, which is related to the deflection angle from the starting position of the starting posture to the second target position of the target posture when the robot moves in a preset trajectory motion mode. As an example, the preset trajectory motion mode can be a straight line, a curve, or a combination of a straight line and a straight line (a broken line), a combination of a straight line and a curve, a combination of a curve and a curve, or other shape trajectory motion in a two-dimensional or three-dimensional space. As an example, the curved motion can include motion along an arc / sphere. In this embodiment, the step S103 may further include: obtaining the second target position of the target posture based on the rotation parameter of the deflection angle obtained from the starting position and the first target position.
[0071] In a further specific example, taking arc / spherical motion as an example, when the motion control model calculates the second target position, a first position vector pointing from the center of the circle / sphere to the starting position can be obtained based on the coordinates of the center of the circle / sphere and the starting position, and a second position vector pointing from the center of the circle / sphere to the second target position can be obtained based on the first position vector and the rotation parameter. Further, the second target position is obtained based on the coordinates of the center of the circle / sphere and the second position vector.
[0072] In some embodiments, the rotation parameters include: at least one rotation matrix constructed by the angle between a first position vector pointing to a starting position on the arc / sphere and a second position vector pointing to a second target position on at least one arc / sphere passing through a preset trajectory.
[0073] Take the one-dimensional admittance control model of equations (1) to (4) to control the two-dimensional linear motion control model as an example.
[0074] In this example, the formula description of the two-dimensional linear motion control model is given:
[0075] f(p)=q (5)
[0076] Among them, p represents the one-dimensional control quantity, that is, the first target position output by the one-dimensional admittance control model.
[0077] q=[q x q y ] T ,q x and q y Represents the abscissa and ordinate in the second coordinate system of the two-dimensional motion control model.
[0078] Combination Figure 2 As shown in the figure, assuming that the robot's origin coordinate in the second coordinate system is O, and the circular trajectory is the target motion trajectory, assuming that the center position C0 of the circular trajectory is expressed in two-dimensional coordinates as [c x c y ] T , the radius is r; further, assuming that the coordinates of the starting point C1 of the circular trajectory are [c x +rc y ] T , the second target position is C2. The specific calculation process of the circular trajectory given by formula (5) is:
[0079]
[0080] in, represents a two-dimensional rotation matrix about the angle θ between the first position vector C0C1 and the second position vector C0C2; Figure 2 In the example, the angle between vectors OC0 and OC1 is angle θ = arc length / radius = p / r. The calculated vector is C0C2, which is obtained by rotating the vector C0C1 by an angle θ, and then summing it with the center position coordinate C0 to obtain the C2 coordinate.
[0081] Combined with the output of the one-dimensional admittance control model as the control quantity, the target position p in equation (4) is n+1 Substituting into formula (6) we get:
[0082]
[0083] The coordinates of the second target position are obtained from formula (7): It can be used as the output of the robot motion control model.
[0084] It is understandable that the circular motion between C1 and C2 is only an example, and C1 and C2 may also be linear motion (such as Figure 2 In the example of the dashed line, let the length be pn+1 '), compared with circular motion, the only difference is the geometric relationship between arc length p, radius r and angle θ. For example, p n+1 '=2rsin(θ / 2), we get θ=2arcsin(p n+1 ' / 2r). It can be seen that when controlling the two-dimensional linear movement by a one-dimensional control amount, p n+1 ' and r are used to describe θ to replace p / r in the above formula.
[0085] In some embodiments, the target posture includes not only the second target position, but also the second target angle at the second target position. In the corresponding example, the motion control model parameter also includes a rotation parameter, and the rotation parameter is related to the ratio of the movement amount from the starting posture to the target posture of the preset trajectory motion mode to the total movement amount from the starting posture to the end posture when the robot rotates during the motion process of the preset trajectory motion mode. For example, assuming that the movement range of the robot's end effector in a two-dimensional plane is limited to a linear motion between points D and E, and the length of DE is M. In the process from D to E, the end effector of the robot will undergo a linear rotation from 1° to 90°, and the robot has a corresponding rotation angle and movement amount (such as DF) at each position F in DE. If F is the midpoint of DE, when the robot reaches the midpoint F of DE, its rotation angle can be 1°+44.5°=45.5°. Similarly, when DE is a motion trajectory of other motion modes, the corresponding rotation angle can also be obtained according to this proportional relationship. Therefore, in step S103, the total rotation angle between the starting angle at the starting position and the ending angle at the ending position can be obtained, and the second target angle of the target posture can be obtained based on the partial angle of the total rotation angle under the proportional relationship.
[0086] Take the three-dimensional mixed motion model as an example. The three-dimensional mixed motion model is assumed to be Figure 2 A one-dimensional rotation is added on the basis of the circular trajectory (not shown).
[0087] Specifically, referring to equations (1) to (4), the control output of the one-dimensional admittance control model can be obtained. Referring to equations (8) to (9), it is assumed that a one-dimensional rotation is added to the two-dimensional circular trajectory, and it is assumed that the center position C0 is expressed in three-dimensional coordinates as [h x h y 0] T , the radius is r, the coordinates of the starting point C1 are [h x +rh y 0] T ; The starting angle of the one-dimensional rotation is The three-dimensional coordinates are expressed as It is denoted as F0, and the end angle is The three-dimensional coordinates are expressed as Denoted as F1, the calculation formula of the three-dimensional hybrid motion model is:
[0088]
[0089] in, represents the three-dimensional rotation matrix about the angle θ, and That is, the rotation coefficient, combined with available It can be understood as the ratio of the rotation angle to the total angle, or the ratio of the corresponding arc length to the circumference of the circular motion (ie, the total movement amount).
[0090] Combined with the output of the one-dimensional admittance control model, the first target position p in equation (4) is n+1 Substituting into formula (8) we get:
[0091]
[0092] in, is the coordinate of the target pose.
[0093] Based on the principles of the above examples, although only an example of using a one-dimensional admittance control model to control a two-dimensional motion control model is given, it can be inferred that in other embodiments, model combinations of other different motion dimensions can also be used, such as a two-dimensional admittance control model to control a three-dimensional, four-dimensional, five-dimensional, or six-dimensional motion control model.
[0094] The following is an example of a two-dimensional admittance model controlling a three-dimensional motion control model, but it should be understood that the present invention is not limited to this example.
[0095] A formula description of a two-dimensional admittance control model is given:
[0096]
[0097] Where M = [M x M y ] T represents the actual external input force vector, represents the target interaction force vector;
[0098] represents the virtual mass parameter matrix;
[0099] represents the virtual damping parameter matrix;
[0100] represents the virtual elastic coefficient matrix;
[0101] p represents the robot’s current velocity vector, speed vector, and position vector respectively;
[0102] p0 represents the robot's preset velocity vector and position vector respectively.
[0103] The target acceleration vector of the control model in the current control cycle is obtained by formula (2): for
[0104]
[0105] in,
[0106] According to formula (11), the control quantity output of the control model is:
[0107] It can be understood that the calculation principle in this embodiment is similar to the principle of formula (1) to formula (4), except that the one-dimensional transformation is made into a two-dimensional transformation.
[0108] Here we first give the formula description of the three-dimensional motion control model:
[0109] f(p)=q (12)
[0110] Where p represents the two-dimensional input parameters of the motion model;
[0111] Next, the sphere is used as the three-dimensional motion control model, assuming that the center position S0 of the sphere is expressed in three-dimensional coordinates as The radius is r, and the coordinates of the starting point S1 are Then the specific calculation process of equation (12) about spherical motion is given as
[0112] f(p)=T 3 (θ y ,θ z )(S1-S0)+S0 (13)
[0113] in, Represents the angle θ y ,θ z The three-dimensional rotation matrix and
[0114] Combined with the one-dimensional admittance control model, the target position p in equation (4) is n+1 Substitute into formula (13) and simplify to obtain:
[0115]
[0116] It can be understood that the principles of equations (11) to (14) are similar to those of equations (6) to (7), except that the two-dimensional circular trajectory motion is changed to motion in a three-dimensional spherical surface, so the coordinate dimension of the corresponding position point and the dimension of the rotation matrix change accordingly.
[0117] Step S104: Control the robot to move to achieve the target posture.
[0118] In some embodiments, through the admittance control model and the multi-dimensional motion model in any of the above embodiments, the corresponding target posture (which may include the coordinates of the second target position, or the coordinates and rotation angle of the second target position) is calculated based on the external force, and corresponding control instructions are generated to the robot's motion execution device (including motor driver, drive motor, actuator, etc.) to enable the motion execution device to perform actions to achieve the target posture.
[0119] Therefore, it is possible to control the high-dimensional motion control model through the low-dimensional admittance control model, thereby controlling the robot motion. Compared with the solution of using the high-dimensional admittance control model, the amount of calculation is effectively reduced.
[0120] like Figure 3 As shown, a schematic diagram of the circuit structure of a control device in an embodiment of the present disclosure is shown.
[0121] The control device 300 may be exemplified as a controller in a robot, or a processing device connected to the robot for communication, such as a server, a desktop computer, a laptop computer, a tablet computer, a smart phone, or other terminal.
[0122] The control device 300 includes a bus 301, a processor 302, and a memory 303. The processor 302 and the memory 303 can communicate with each other through the bus 301. The memory 303 can store program instructions. The processor 302 implements the steps of the robot motion control method in the previous embodiment by running the program instructions in the memory 303, for example Figure 1 shown.
[0123] The bus 301 may be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, although only one thick line is used in the figure, it does not mean that there is only one bus or one type of bus.
[0124] In some embodiments, the processor 302 may be implemented as a central processing unit (CPU), a microprocessing unit (MCU), a system on chip (System On Chip), or a field programmable logic array (FPGA). The memory 303 may include a volatile memory (Volatile Memory) for temporary storage of data when running a program, such as a random access memory (Random Access Memory, RAM).
[0125] The memory 303 may also include a non-volatile memory (non-volatile memory) for data storage, such as a read-only memory (ROM), a flash memory, a hard disk drive (HDD) or a solid-state disk (SSD).
[0126] In some embodiments, the control device 300 may further include a communication interface 304. The communication interface 304 is used to communicate with the outside. In a specific example, the communication interface 304 may include one or a group of wired and / or wireless communication circuit modules. For example, the communication interface 304 may include one or more of a wired network card, a USB module, a serial interface module, etc. The wireless communication protocols followed by the wireless communication module include: for example, near field communication (NFC) technology, infrared (IR) technology, global system for mobile communications (GSM), general packet radio service (GPRS), code division multiple access (CDMA), wideband code division multiple access (WCDMA), time division code division multiple access (TD-SCDMA), long term evolution (LTE), Bluetooth (BT), global navigation satellite system (GNSS), etc. One or more.
[0127] like Figure 4As shown, a schematic diagram of the structure of a robot in one embodiment of the present disclosure is shown.
[0128] The robot 400 includes: a collection device 401 , a motion execution device 402 and a control device 403 .
[0129] The acquisition device 401 may include a force sensor for acquiring the external force exerted on the end effector of the robot 400 .
[0130] The motion execution device 402 may include an execution mechanism that is connected to and drives the end effector to perform multi-dimensional motion, and the multi-dimensional motion may be sliding, rotating, etc. As an example, the execution mechanism may include one or more sliding mechanisms, such as a combination of a slide rail / slide groove and a slider, a screw rod and a slider screwed thereto, etc., and may also include one or more shaft rotation mechanisms to achieve rotation.
[0131] As an example, the motion execution device 402 may include: a motor driver including a motion controller (eg, a PID controller), a current acquisition and control circuit, and a drive motor (eg, a DC motor or an AC motor).
[0132] The control device 403 can be based on Figure 3 The control device 403 in the example may be implemented as a controller built into the robot 400, or as a processing terminal connected to the robot 400. The memory of the control device 403 may pre-store the low-dimensional admittance control model and the high-dimensional motion control model to be called when executing the method.
[0133] The control device 403 can control the motor driver according to the target posture to drive the drive motor to drive the connected actuator (such as the end effector) to perform movement and force output to the user. For example, actively drive the user to move, or provide the user with a pre-designed interactive force during passive movement, such as setting a resistance for rehabilitation training for the user.
[0134] like Figure 5 As shown, a schematic diagram showing the principle of robot admittance motion control in one embodiment of the present disclosure is shown.
[0135] exist Figure 5 In the figure, the signal acquisition device 401 of the robot 400 is shown to collect the external force applied by the user to the robot 400 and output the original signal of the external force. Preferably, the robot 400 also includes a signal processing circuit 404 for processing the original signal to obtain a quantized signal. For example, the original signal is subjected to a processing method such as noise reduction and filtering, and a signal quantization method (such as image processing, such as grayscale processing, etc.) is performed to obtain a quantized signal that can be recognized by the control device 403.
[0136] The admittance control model in the control device 403 generates a motion quantity characteristic as a control quantity, such as a first target position, according to the quantized signal of the external force; the motion control model obtains a target posture according to the control quantity. The control device 403 can generate a motion control signal corresponding to the target posture and send it to the motion execution device 402. As an example, the motion control signal can include posture information of the target posture, such as the coordinates of the second target position and target angle information. Of course, the motion control signal can also include the movement amount, rotation angle amount, etc. that the motion execution device 402 needs to execute to make the robot 400 achieve the target posture.
[0137] The motion execution device 402 controls the robot 400 to achieve the target posture according to the control signal.
[0138] like Figure 6 As shown, a schematic diagram of a module of a motion control device for a robot in an embodiment of the present disclosure is shown. It should be noted that the principle and technical implementation of the motion control device can refer to the motion control method in the previous embodiment, so it will not be repeated in this embodiment.
[0139] exist Figure 6 In the embodiment, the motion control device comprises:
[0140] The acquisition module 601 is used to obtain the external force applied to the robot in the current control cycle;
[0141] An admittance control module 602 is used to determine the motion characteristics of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system; the motion characteristics include a first target position and / or a first target angle;
[0142] A motion control module 603 is used to determine a target posture of a current control cycle achieved by the target motion amount through a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, with the motion amount feature as the control amount, mapping the control amount into a target motion amount in the second coordinate system according to a preset trajectory motion mode, and determining the target posture including a second target position or a second target position and a second target angle.
[0143] The motion execution control module 604 is used to control the robot motion to achieve the target posture.
[0144] It should be noted that in Figure 6Each functional module in the embodiment can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented by software, it can be implemented in whole or in part in the form of a program instruction product. The program instruction product includes one or a group of program instructions. When the program instruction is loaded and executed on the control, the process or function according to the present disclosure is generated in whole or in part. The control can be a general control, a dedicated control, a control network, or other programmable devices. The program instructions can be stored in a control readable storage medium, or transmitted from one control readable storage medium to another control readable storage medium.
[0145] and, Figure 6 The device disclosed in the embodiment can be implemented by other module division methods. The device embodiments shown above are only schematic. For example, the division of the modules is only a logical function division. There may be other division methods in actual implementation, such as a group of modules or modules can be combined or dynamically added to 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, and the indirect coupling or communication connection of devices or modules can be electrical or other forms.
[0146] in addition, Figure 6 Each functional module and submodule in the embodiment can be dynamically in a processing component, or each module can exist physically separately, or two or more modules can be dynamically in one component. The above-mentioned dynamic components can be implemented in the form of hardware or in the form of software functional modules. If the above-mentioned dynamic components are implemented in the form of software functional modules and sold or used as independent products, they can also be stored in a control readable storage medium. The storage medium can be a read-only memory, a disk or an optical disk, etc.
[0147] It should be particularly noted that the process or method represented by the flowchart of the above embodiment of the present disclosure can be understood as representing a module, fragment or part of a code including one or more sets of executable instructions configured to implement the steps of a specific logical function or process. And the scope of the preferred embodiment of the present disclosure includes other implementations, in which the functions may not be performed in the order shown or discussed, including performing the functions in a substantially simultaneous manner or in a reverse order according to the functions involved.
[0148] For example, Figure 1 The order of the steps in the method embodiments may be changed in specific scenarios and is not limited to the above.
[0149] In an embodiment of the present disclosure, a computer-readable storage medium may be provided, storing program instructions, which, when executed, implement the steps of the robot motion control method in any of the previous embodiments.
[0150] That is, the method steps in the above embodiments are implemented as software or control codes that can be stored in a recording medium (such as a CD ROM, RAM, floppy disk, hard disk or magneto-optical disk), or are implemented as control codes originally stored in a remote recording medium or a non-temporary machine-readable medium downloaded via a network and to be stored in a local recording medium, so that the method represented herein can be stored in such software processing on a recording medium using a general-purpose control, a dedicated processor, or programmable or dedicated hardware (such as an ASIC or FPGA).
[0151] A computer program product may also be provided in an embodiment of the present disclosure, comprising: program instructions for executing the steps in the robot motion control method in any of the previous embodiments.
[0152] In summary, the present disclosure provides a motion control method, a robot, a storage medium and a program product for a robot, wherein the method comprises: obtaining an external force on the robot in the current control cycle; determining the motion quantity characteristics of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system; the motion quantity characteristics include at least one of the following: a first target position, a first movement amount and a first target angle; using a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, taking the motion quantity characteristics as the control quantity, mapping the control quantity to the target motion quantity in the second coordinate system according to a preset trajectory motion mode, determining the target posture of the current control cycle achieved by the target motion quantity; and controlling the robot to achieve the target posture. Thus, the high-dimensional motion control model can be controlled by a low-dimensional admittance control model, and the amount of calculation can be effectively reduced while ensuring precise motion control. .
[0153] The above embodiments are merely illustrative of the principles and effects of the present disclosure, and are not intended to limit the present disclosure. Anyone familiar with the technology may modify or change the above embodiments without violating the spirit and scope of the present disclosure. Therefore, all equivalent modifications or changes made by those with ordinary knowledge in the technical field without departing from the spirit and technical ideas disclosed in the present disclosure shall still be covered by the protection scope of the present disclosure.
Claims
1. A robot motion control method, characterized in that: include: In the current control cycle, obtain the external force on the robot; Determining the motion characteristics of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system; the motion characteristics include a first target position and / or a first target angle; By using a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, taking the motion quantity feature as the control quantity, mapping the control quantity into a target motion quantity according to a preset trajectory motion mode in the second coordinate system, and determining a target posture of the current control cycle achieved by the target motion quantity, wherein the target posture includes a second target position or a second target position and a second target angle; Control the robot to move to achieve the target posture.
2. The robot motion control method according to claim 1, characterized in that: The admittance control model includes: a determined relationship between a comprehensive result of one or more physical parameters of the robot's inertia, damping, elasticity, and nonlinear force and the residual force after the external force overcomes the preset interaction force of the robot; the inertia is related to the first target acceleration of the robot; the damping is related to the speed change of the first current speed of the robot relative to the preset speed; The elasticity is related to a position change of the robot's first current position relative to a preset position.
3. The robot motion control method according to claim 2, characterized in that: The motion characteristic includes a first target position, and determining the motion characteristic of the robot in the first coordinate system based on the external force through an admittance control model related to the first coordinate system includes: determining the first target acceleration according to the first current speed and the first current position by using the admittance control model; Determining a first target speed based on the first current speed, a first target acceleration, and a current control cycle duration; The first target position is determined based on the first current position, the first target speed, and the current control cycle duration.
4. The robot motion control method according to claim 1, characterized in that: The motion control model includes: a calculation relationship of how the starting posture of the preset trajectory motion mode reaches the target posture under the action of motion control model parameters; the starting posture includes a starting position and a starting angle; and the motion control model parameters are determined by the preset trajectory motion mode.
5. The robot motion control method according to claim 4, characterized in that: The motion control model parameters include: a rotation parameter related to a deflection angle from a starting position of the starting posture to a second target position of the target posture when the robot moves in a preset trajectory motion mode; The method uses a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, takes the motion quantity feature as the control quantity, maps the control quantity into a target motion quantity in the second coordinate system according to a preset trajectory motion mode, and determines a target posture of the current control cycle achieved by the target motion quantity, including: Based on the starting position and the rotation parameter of the deflection angle obtained from the first target position, a second target position of the target posture is obtained.
6. The robot motion control method according to claim 5, characterized in that: The target posture also includes a second target angle; the motion control model parameters also include: a rotation parameter, which is related to the ratio of the deflection angle from the starting posture to the target posture of the preset trajectory motion mode to the total deflection angle from the starting posture to the end posture when the robot rotates in the preset trajectory motion mode; The method uses a linear motion control model related to a second coordinate system having a higher dimension than the first coordinate system, takes the motion quantity feature as the control quantity, maps the control quantity into a target motion quantity in the second coordinate system according to a preset linear motion mode, and determines a target posture of the current control cycle achieved by the target motion quantity, and further includes: The total rotation angle between the starting angle at the starting position and the ending angle at the ending position is obtained, and based on the partial angle of the total rotation angle under the proportional relationship, a second target angle of the target posture is obtained.
7. The robot motion control method according to claim 6, characterized in that: The rotation parameters include: at least one rotation matrix constructed by the angle between a first position vector pointing to a starting position on the arc / sphere and a second position vector pointing to a second target position on the at least one arc / sphere passing through a preset trajectory motion.
8. The robot motion control method according to claim 5, characterized in that: The preset trajectory motion mode includes circular arc / spherical trajectory motion; The method uses a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, takes the motion quantity feature as the control quantity, maps the control quantity into a target motion quantity in the second coordinate system according to a preset trajectory motion mode, and determines a target posture of the current control cycle achieved by the target motion quantity, including: The motion control model obtains a first position vector pointing from the center of the circle / sphere to the starting position based on the center of the circle / sphere and the starting position coordinates, obtains a second position vector pointing from the center of the circle / sphere to the second target position based on the first position vector and the rotation parameters, and obtains the second target position based on the coordinates of the center of the circle / sphere and the second position vector.
9. The robot motion control method according to claim 6, characterized in that: The preset trajectory motion mode includes circular arc / spherical motion; The method uses a motion control model related to a second coordinate system with a higher dimension than the first coordinate system, takes the motion quantity feature as the control quantity, maps the control quantity into a target motion quantity in the second coordinate system according to a preset trajectory motion mode, and determines a target posture of the current control cycle achieved by the target motion quantity, including: The target angle is obtained based on the change angle obtained under the proportional relationship based on the angle between the starting angle of the starting posture and the ending angle of the ending posture, and based on the sum of the change angle and the starting angle.
10. The robot motion control method according to claim 1, characterized in that: The first coordinate system is a one-dimensional or multi-dimensional coordinate system including at least one movement dimension and / or at least one rotation dimension, and the second coordinate system is a multi-dimensional coordinate system including multiple movement dimensions or multiple movement dimensions and at least one rotation dimension.
11. A robot, characterized in that: include: A collection device, used for collecting the external force applied to the robot; A motion execution device, used to execute the action; Control device, including: A processor, a memory and a communication interface; the communication interface is communicatively coupled to the acquisition device and the motion execution device; The memory stores program instructions; The processor is used to run the program instructions to execute the robot motion control method as described in any one of claims 1 to 10, generate control instructions and send them to the motion execution device through the communication interface, so that the motion execution device performs actions according to the control instructions to achieve the target posture.
12. A computer-readable storage medium, characterized in that: Program instructions are stored, and when the program instructions are executed, the robot motion control method according to any one of claims 1 to 10 is executed.
13. A computer program product, characterized in that include: Used to execute the motion control method of the robot according to any one of claims 1 to 10.
