WO2022205940A1 - 机器人运动控制方法、装置和机器人 - Google Patents
机器人运动控制方法、装置和机器人 Download PDFInfo
- Publication number
- WO2022205940A1 WO2022205940A1 PCT/CN2021/131890 CN2021131890W WO2022205940A1 WO 2022205940 A1 WO2022205940 A1 WO 2022205940A1 CN 2021131890 W CN2021131890 W CN 2021131890W WO 2022205940 A1 WO2022205940 A1 WO 2022205940A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- joint
- robot
- angle
- limit
- avoidance
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 230000001133 acceleration Effects 0.000 claims abstract description 102
- 238000005457 optimization Methods 0.000 claims abstract description 87
- 230000014509 gene expression Effects 0.000 claims description 51
- 239000011159 matrix material Substances 0.000 claims description 49
- 230000006870 function Effects 0.000 claims description 38
- 230000008859 change Effects 0.000 claims description 32
- 238000013507 mapping Methods 0.000 claims description 22
- 230000004913 activation Effects 0.000 claims description 18
- 238000004590 computer program Methods 0.000 claims description 12
- 238000009499 grossing Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000010276 construction Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000006399 behavior Effects 0.000 description 6
- 238000013459 approach Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002620 method output Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1612—Programme controls characterised by the hand, wrist, grip control
Definitions
- the present application relates to the technical field of robot control, and in particular, to a robot motion control method, device and robot.
- robots are gradually moving out of a structured environment and into a dynamic and changeable unstructured environment.
- complex tasks mechanical arm motion following, footed robot mass center momentum control, etc.
- various external disturbances avoiding obstacle collision, compliant response to external force dragging, etc.
- joint overruns may occur. bit problem.
- how to ensure that when the joint angle is close to the joint limit, can be smoothly away from the joint limit when the robot performs a task, is the key to realize the safe and stable operation of the robot in an unstructured environment.
- the embodiments of the present application provide a robot motion control method, device, and robot, which can achieve stable distance from joint limits, and still ensure that the original task can be smoothly performed when the degree of freedom is sufficient, so that the robot can run safely and stably .
- Embodiments of the present application provide a method for controlling motion of a robot, including:
- the expected motion state information of each joint of the robot for performing the task in the corresponding control period is calculated, and the joint angle related information includes each joint angle;
- the virtual repulsion force of each joint away from the limit corresponding to the control period is calculated, and the information related to the limit avoidance angle of each joint is calculated according to the virtual repulsion force;
- the optimal solution for the joint smooth limit avoidance optimization model is obtained, so as to obtain the joint angular acceleration that satisfies the joint smooth limit avoidance and is used to perform the task or joint angular velocity;
- Joint control instructions corresponding to control periods are generated by using the joint angular acceleration or joint angular velocity, and the joint control instructions in each control period are used to control the motion of the robot to complete the planned task.
- the expected motion state information of each joint of the robot for performing the task in the corresponding control period is calculated, including:
- the desired motion state information of each joint of the robot for performing the task in the corresponding control period is calculated based on the preset joint control law.
- the expression of the virtual repulsion force is pre-built based on the joint angle of the joint and the range of the joint limit angle, including:
- the virtual repulsion force When the actual feedback joint angle is within the angle safety range of the joint, the virtual repulsion force is 0; when the actual feedback joint angle is outside the joint angle safety range, the closer the joint angle is to the joint angle In the case of the upper limit or lower limit of the joint limit angle range, the virtual repulsion force is larger and the direction is farther from the joint limit direction.
- the information related to the limit avoidance angle includes the limit avoidance angle of the joint, and the relationship between the limit avoidance angle of the joint and the virtual repulsion force is constructed based on the joint admittance control. as follows:
- ⁇ q JAL represents the limit avoidance angle of the joint
- ⁇ JAL represents the virtual repulsion
- M q , D and K are the virtual inertia matrix, virtual damping matrix and virtual stiffness matrix based on joint admittance control respectively
- s represents the differential operator.
- the joint smoothness and limit avoidance optimization model is pre-built, including:
- a smooth limit avoidance optimization model is constructed based on the simultaneous equation of the mapping equation and the expression of the joint angular velocity or the joint angular acceleration change, wherein the optimization variable of the smooth limit avoidance optimization model is related to the joint angular velocity or the joint angular acceleration
- the constraints of the smooth limit avoidance optimization model are constructed based on the equality constraints and/or inequality constraints that the robot needs to satisfy during the task execution process.
- the expression for constructing the joint angular velocity or joint angular acceleration variation based on the relevant information of the joint's avoidance angle includes:
- An expression for the change of the joint angular velocity or the joint angular acceleration is constructed according to the relevant information of the avoidance angle and the projection matrix.
- the information related to the limit avoidance angle further includes the limit avoidance angular acceleration of the joint, and the desired motion state information includes the desired acceleration of the joint;
- the joint equation based on the optimization variable related to the joint angular acceleration is constructed based on the simultaneous equation of the mapping equation and the expression of the joint angular acceleration change. Smooth and avoid limit optimization model;
- mapping equation is:
- J is the Jacobian matrix; represents the joint angular acceleration to be solved; represents the desired acceleration of the joint in the task space; is the derivative of the Jacobian matrix J; is the actual feedback joint angular velocity;
- the smoothing and limit avoidance optimization model is constructed using a convex quadratic programming optimization function, and the expression of the convex quadratic programming optimization function is:
- both C and d are coefficient matrices of inequality constraints, which are solved according to the equality constraints and/or inequality constraints that the robot needs to satisfy during the task execution process;
- v 1 represents the first value of the equality constraints and/or inequality constraints.
- slack variable
- the limit avoidance angle related information further includes the limit avoidance angular velocity of the joint, and the desired motion state information includes the desired speed of the joint;
- the joint type of the robot is a position-controlled joint, then construct a smooth limit avoidance optimization model based on the optimization variables related to the joint angular velocity based on the simultaneous equation of the mapping equation and the expression of the joint angular velocity change;
- mapping equation is:
- J is the Jacobian matrix; represents the joint angular velocity to be solved; represents the desired velocity of the joint in the task space;
- I is the unit matrix
- ⁇ is the projection matrix
- ⁇ 2 represents the increment of the joint's angular velocity.
- the smoothing and limit avoidance optimization model is constructed using a convex quadratic programming optimization function, and the expression of the convex quadratic programming optimization function is:
- C and d are both inequality-constrained coefficient matrices, which are solved according to the equality constraints and/or inequality constraints that the robot needs to satisfy during the task execution process;
- v 2 represents the second equality constraints and/or inequality constraints slack variable.
- the embodiment of the present application also provides a robot motion control device, including:
- the expected value calculation module is used to calculate the expected motion state information of each joint of the robot in the corresponding control period for performing the task according to the planned task trajectory information and the joint angle related information fed back by the robot in the corresponding control period, and the joint angle related information includes: each joint angle;
- a limit avoidance angle information calculation module configured to solve the virtual repulsion force of each joint away from the limit corresponding to the control cycle based on the feedback of each joint angle, and calculate the limit avoidance angle related information of each joint according to the virtual repulsion force;
- the optimization model solving module is used to optimally solve the joint smooth limit avoidance optimization model based on the desired motion state information of the corresponding control period and the limit avoidance angle related information, so as to obtain the joint smooth limit avoidance and use Joint angular acceleration or joint angular velocity for performing the task;
- the control instruction execution module is used for generating joint control instructions corresponding to control periods by using the joint angular acceleration or joint angular velocity, and the joint control instructions in each control period are used to control the motion of the robot to complete the planned task.
- An embodiment of the present application further provides a robot, the robot includes a processor and a memory, the memory stores a computer program, and the processor is configured to execute the computer program to implement the above-mentioned robot motion control method.
- Embodiments of the present application further provide a readable storage medium, which stores a computer program, and when the computer program runs on a processor, implements the above-mentioned method for controlling the motion of a robot.
- the robot motion control method of the embodiment of the present application solves the information related to the limit avoidance angle by constructing the virtual repulsion force generated by the joint in the direction away from the limit and combining the joint admittance control, and also constructs the optimization problem of smooth limit avoidance. Solving the control instructions of each joint that can smoothly avoid the limit of the joint to control the robot to perform the planning task, the robot can smoothly generate the behavior away from the joint limit, and when the degrees of freedom are satisfied, the original planning task can still be guaranteed. The smooth execution makes the robot run safely and stably. This method does not need to plan the collision avoidance trajectory, and can make the robot generate reflective collision avoidance behavior in real time, so as to ensure the safe and stable operation of the robot from the software algorithm level.
- FIG. 1 shows a schematic flowchart of a robot motion control method according to an embodiment of the present application
- FIG. 2 shows a schematic flowchart of the construction of the joint smoothing and avoiding limit optimization model of the robot motion control method according to the embodiment of the present application
- FIG. 3 shows a schematic flowchart of the desired motion state information acquisition of the robot motion control method according to the embodiment of the present application
- FIG. 4 shows a schematic flowchart of the expression construction of the joint angular velocity or joint angular acceleration variation of the robot motion control method according to the embodiment of the present application
- FIG. 5 shows a schematic structural diagram of a robot motion control apparatus according to an embodiment of the present application.
- the existing methods have the following shortcomings:
- For the first motion planning method it is difficult to predict when the robot has many degrees of freedom and external disturbances (such as the impact trajectory of obstacles or the size and direction of disturbances such as external force dragging in a period of time in the future) are difficult to predict.
- this motion planning method will be difficult to generate a new limit avoidance trajectory in real time and online, and it is difficult to use this kind of method for limit avoidance at this time.
- the second method of motion truncation although it can prevent the occurrence of collision, it will cause sudden changes in the acceleration, speed and position commands of each joint of the robot, which will affect the stability of the robot's operation and the effect of task execution, and even cause the robot to vibrate violently or suddenly. Waiting phenomenon.
- an embodiment of the present application proposes a robot motion control method, which does not need to plan collision avoidance trajectories in advance, but can smoothly generate behaviors away from the joint limit, and optimize the obtained results when the degrees of freedom are satisfied.
- the joint control instructions can still ensure the smooth execution of tasks and make the robot run safely and stably. The method will be described below with reference to specific embodiments.
- this embodiment proposes a robot motion control method, which is applicable to robots with joint types such as position-controlled joints, force-controlled joints, and force-position hybrid control types.
- This method is a general motion control method. , suitable for motion control of robotic arms, humanoid robots, multi-legged robots, etc.
- Step S10 according to the planned task trajectory information and the joint angle related information fed back by the robot in the corresponding control period, calculate the expected motion state information of each joint of the robot for executing the task in the corresponding control period.
- the above-mentioned task trajectory is usually obtained by pre-planning, as the movement reference trajectory of the robot in the task space when the robot performs the desired task, the task trajectory can refer to a reference trajectory for a period of time including multiple control cycles, or it can be a corresponding trajectory.
- the reference trajectory of the control period is not limited here.
- the task trajectory information may include, but is not limited to, a reference position, a reference speed, and a reference acceleration for the robot to perform the planned task.
- the joint angle related information mainly refers to the real-time feedback of the robot in each control cycle of the actual joint angle, joint angular velocity and/or joint angular acceleration and other information in the joint space.
- it can be acquired by sensors such as angle encoders.
- the robot can also obtain other state feedback information as required, which is not limited here.
- the external force actually received by the end of the robot can be measured by a force sensor or a torque sensor such as a force sensor installed at the end of the robot arm, or the body posture of the robot can be obtained by measuring, such as an inertial measurement unit (IMU), etc. Wait.
- IMU inertial measurement unit
- the expected value of each joint of the robot in the task space can be obtained by using the actual state information fed back by the robot in real time and the planned reference value in combination with the feedback control technology.
- the actual state information obtained by the real-time feedback is the relevant information of the robot in the joint space, which needs to be converted into the corresponding information in the task space before the operation with the reference value.
- step S20 the virtual repulsion force of each joint away from the limit corresponding to the control period is obtained based on the feedback of each joint angle, and the information related to the limit avoidance angle of each joint is calculated according to the virtual repulsion force.
- the virtual repulsion force is a virtual force or moment defined by the actual joint angle when it approaches the joint limit angle and is used to repel or move away from the joint limit, and its direction is the direction away from the joint limit.
- the virtual repulsive force can refer to a virtual repulsion force or a virtual repulsive moment.
- a virtual repulsive force is constructed for a translational joint; since a moment cannot be generated, a virtual repulsive force is constructed; for a rotary joint, since a rotational moment can be generated, The virtual repulsion moment is constructed at this time.
- a virtual repulsion force related to the joint angle and the joint limit angle range can be constructed in advance.
- the joint limit angle range refers to the maximum angle range that the joint can reach, and the limit angle range is formed by the upper angle limit and the lower angle limit, and its size often depends on the actual mechanical structure design of the robot.
- the joint angle usually has a safe rotation angle range before reaching the upper limit or lower limit of the limit angle, no virtual repulsion is usually generated at this time, so the angular safety range of the joint is also set, that is, within the angle safety range
- the inner joint can be turned at any angle without obstruction.
- the virtual repulsion force when constructing the virtual repulsion force, it should be satisfied that: when the actual feedback joint angle is within the angle safety range of the joint, the virtual repulsion force is 0; when the actual feedback joint angle is outside the joint angle safety range, Then, when the joint angle is closer to the upper limit or lower limit of the joint limit angle range, the virtual repulsion force generated at this time is larger and the direction is away from the joint limit direction.
- the above-mentioned information related to the avoidance angle may include the avoidance angle of the joint, the avoidance angular velocity and/or the avoidance angular acceleration, and the like.
- the limit avoidance angle refers to the joint angle away from the joint limit.
- the limit avoidance angular velocity and the limit avoidance angular acceleration respectively refer to the joint angular velocity away from the joint limit and the joint angular acceleration away from the joint limit.
- the relationship between the virtual repulsion force and the limit avoidance angle can be constructed in advance.
- the relationship between the joint angle and the limit avoidance angle of the joint can also be constructed in other ways, which is not limited here.
- the closer the joint angle is to the joint limit the greater the virtual repulsion force will be, and thus the greater the limit avoidance angular velocity and the limit avoidance angular acceleration will be.
- step S20 exemplarily, after each joint angle of the real-time feedback corresponding to the control period is obtained, the virtual repulsion force of each joint in the corresponding control period can be calculated by substituting it into the expression of the virtual repulsion force, and then the virtual repulsion force is used.
- the relational expression between the limit avoidance angle and the limit avoidance angle is used to calculate the limit avoidance angle related information of each joint in the corresponding control period, such as the limit avoidance angular velocity and the limit avoidance angular acceleration.
- Step S30 based on the desired motion state information of the corresponding control period and the relevant information of the limit avoidance angle, optimally solve the joint smooth limit avoidance optimization model, so as to obtain the joint smooth limit avoidance and used for performing the task.
- Joint angular acceleration or joint angular velocity
- a joint smooth limit avoidance optimization model is constructed in advance, that is, a smooth limit avoidance optimization problem is constructed, and the optimization model adopts the joint angular acceleration or joint angular velocity.
- Relevant parameters are used as optimization variables, for example, the joint angular acceleration or the increment of joint angular velocity (also called adjustment amount or change increment) and the like.
- the joint control command obtained based on the optimal joint angular velocity or joint angular acceleration satisfies the requirements of smoothly generating the joint away from the joint.
- Limits, and in the case of satisfying degrees of freedom, can ensure that the robot can successfully perform the planned tasks.
- the pre-construction of the smooth and limit avoidance optimization model includes:
- Step S310 constructing a mapping equation from the joint angle related information of the joint to the desired motion state information.
- a mapping equation between the actual feedback joint angle-related information and the expected motion state information in the task space can be constructed.
- a mapping equation based on the acceleration level can be constructed; for force-controlled joints, since acceleration is not required to calculate the joint torque, a mapping equation based on the velocity level can be constructed.
- Step S320 constructing an expression for the change of the joint angular velocity or the joint angular acceleration based on the relevant information of the joint's avoidance angle.
- the joint formula between the joint angular velocity or joint angular acceleration and the limit avoidance angle information can be pre-built, so as to facilitate the solution of the joint angular velocity or joint angular acceleration change in each control period.
- this embodiment will reflect the changes of the degrees of freedom of the joints by constructing a projection matrix, and the projection matrix will be used to optimize the construction of the model.
- an activation factor related to the joint angle can be constructed based on the angle safety range of the joint, and then the projection matrix is constructed by using the activation factor as a diagonal element.
- the above-mentioned steps S310 and S320 do not limit the order of execution, and the two may be executed simultaneously or in a corresponding order.
- Step S330 based on the mapping equation and the simultaneous equation of the expression of the joint angular velocity or joint angular acceleration variation, construct a joint smoothing and limit avoidance optimization model based on parameters related to the joint angular velocity or joint angular acceleration as optimization variables.
- mapping equation and the expression for the change of joint angular velocity or angular acceleration are combined, so that the two simultaneous equations can be obtained.
- an optimization problem about realizing joint smoothness and avoiding limit can be constructed.
- the smooth limit avoidance optimization model will take the increment of the joint angular velocity or the joint angular acceleration as the optimization variable, of course, there may also be other variables, which are not limited here.
- the constraints of the optimization problem of the joint smoothness avoiding limit are also constructed based on the equality constraints and/or inequality constraints that the robot needs to satisfy during the task execution process. It can be known that the optimal joint angular velocity or joint angular acceleration can be obtained by calculating the optimal solution of the optimization model.
- the optimization model for smoothing and avoiding the limit of the joint can be constructed by, for example, a convex quadratic programming (quadratic programming, QP) optimization function, etc.
- QP quadratic programming
- other optimization functions can also be used or based on the The construction of the optimization function constructed by adaptively adjusting the QP optimization function, etc., shall fall within the protection scope of the present application.
- an optimization model based on an acceleration level or an optimization model based on a velocity level can be determined according to different types of joints.
- the parameters related to the joint angular acceleration can be selected as the optimization variables
- the parameters related to the joint angular velocity can be selected as the optimization variables.
- the optimal joint angular velocity increment can be obtained through the above-mentioned smooth limit avoidance optimization model, and then it can be reversely substituted into the corresponding joint angular velocity or joint angular acceleration change expression, and the optimal joint angular velocity can be calculated.
- optimal joint angular acceleration or joint angular velocity Since the above optimization model includes the projection matrix, that is, the change of the degrees of freedom of the joints is considered, it can be known that when a certain degree of freedom direction is not occupied, the tasks to be performed will not be affected by the limit avoidance motion.
- the subsequent tasks When the degree of freedom in this direction is gradually occupied, the subsequent tasks will gradually use the remaining degrees of freedom (not exceeding the joint limit) to perform as much as possible, thereby giving priority to ensuring the smooth joint limit avoidance function, and at the same time when the degree of freedom is sufficient. In the case of , it can still effectively ensure that the required tasks are completed by the remaining joints within the safe range.
- Step S40 using the joint angular acceleration or joint angular velocity to generate joint control instructions corresponding to the control period, and the joint control instructions of each control period are used to control the motion of the robot to complete the planned task.
- the joint angular velocity command and the joint angle command can be obtained through numerical integration; furthermore, the joint torque command can be obtained by combining with the dynamic model. If the optimal joint angular velocity is obtained, the position command of the joint can be obtained through numerical integration. Further, the calculated corresponding joint control instructions are sent to the corresponding joints to control the robot to perform the current task. It can be understood that if the current control cycle finishes executing the instruction, it will enter the next control cycle, and the above-mentioned steps S10 to S40 will be repeated in each control cycle until the robot completes the required task.
- the above-mentioned robot motion control method by constructing virtual repulsion force and combining with joint admittance control, enables the robot to perform the original task, even if it is difficult to predict the external disturbance in the future, the robot can still be controlled according to the current control cycle. state, and smoothly generate the behavior away from the joint limit, that is, to achieve smooth limit avoidance.
- the degrees of freedom of the joints close to the limit are gradually occupied by the limit avoidance motion, and the tasks that the robot needs to perform gradually become the degrees of freedom formed by the remaining joints that are not beyond the limit.
- the joint when the joint exceeds the limit, the joint avoids the limit first, and the remaining degrees of freedom are used to ensure the smooth execution of the original task as much as possible, and when the joint does not exceed the limit, all the degrees of freedom are used to give priority to the execution of the required Task.
- the robot motion control method in this embodiment does not need to plan a collision avoidance trajectory, so that the robot can generate a reflective collision avoidance behavior in real time, and the robot can run safely and stably from the software algorithm level.
- the method outputs the torque, speed and position commands at the joint end, which is suitable for the series/parallel configuration robot of position control, force control joint and force-position hybrid control joint, and is suitable for manipulators in various fields such as industry, service and specialties.
- quadruped robot, motion control of humanoid robot is a general motion control method.
- this embodiment proposes a method for controlling motion of a robot, which also includes the foregoing steps S10 to S40 . These steps are described in detail below.
- step S10 "calculate the desired motion state information of each joint of the robot for performing the task in the corresponding control period according to the planned task trajectory information and the joint angle-related information fed back by the robot in the corresponding control period", As shown in Figure 3, including:
- step S110 the reference motion state information of the robot in the task space corresponding to the control period is obtained according to the planned task trajectory information.
- the motion trajectory information of the robot in the task space can be pre-planned.
- the motion trajectory can be the motion trajectory of the end of the robot arm, the trajectory of the robot's center of mass, or the rotation trajectory of the upper body posture of the humanoid robot, etc. , which can be determined according to the actual task performed.
- the reference motion state information may include one or more of a reference position, a reference velocity, and a reference acceleration of the robot in the task space in a corresponding control period.
- step S120 the joint angle related information fed back by the robot in the corresponding control period is acquired, and the actual motion state information of each joint of the robot in the task space is calculated according to the joint angle related information.
- the joint angle related information fed back in real time mainly includes the angle of each joint (ie joint angle), angular velocity (ie joint angular velocity) and angular acceleration (ie joint angular velocity).
- the angle of the corresponding joint can be collected through the angle encoder installed at each joint, and then the joint angle can be differentiated, and the corresponding joint angular velocity and joint angular acceleration can be calculated.
- the angle-related information of the joint space is obtained through the sensor measurement, so the actual motion state information corresponding to the robot in the task space can be calculated through forward kinematics.
- the actual motion state information mainly includes the actual position, actual speed and actual acceleration, etc. It should be noted that the above-mentioned steps S110 and S120 do not strictly limit the order of execution, and the two may be executed simultaneously or in a corresponding order.
- Step S130 according to the reference motion state information and the actual motion state information in the task space, calculate the desired motion state information of each joint of the robot for performing the task in the corresponding control period based on the preset joint control law.
- the above-mentioned preset joint control law is also called a feedback control law.
- it may be set at the factory of the robotic arm or pre-generated, or it may be preset by the user according to actual needs, and its preset form is not limited.
- the preset joint control law satisfies:
- x act and represent the actual position and actual speed, respectively;
- x ref and respectively represent the reference position, reference velocity and reference acceleration;
- K p and K d are the position gain and velocity gain in the joint control law, respectively;
- the desired velocity can be obtained by integrating the desired acceleration and desired location It can be understood that the joint control law is only an example, and the joint control law can also be adjusted according to actual requirements in practical applications.
- the desired motion state information corresponding to the control period can be obtained based on the preset joint control law.
- the current desired motion state information may include desired position, desired velocity, and/or desired acceleration, and the like.
- the angle safety range and virtual repulsion of the joint are
- the upper and lower boundaries of the angle safety range of the joint can be further determined according to the joint limit angle range.
- the angle safety range of the joint and the joint limit angle range satisfy the following relationship:
- the proportional coefficient ⁇ can be set according to engineering experience. Of course, other methods can also be used to select the angle safety range, which is not limited here.
- ⁇ max is the maximum joint moment. It can be understood that the i-th joint here is only described as a rotational joint. In the case of a translational joint, the maximum joint moment ⁇ max may be replaced by the maximum joint force f max .
- step S20 "calculating the information related to the avoidance angle of each joint according to the virtual repulsion force"
- the relationship between the virtual repulsion force and the joint angle and the limit avoidance angle of the joint is
- the relationship between the virtual repulsion force and the joint angle and the joint's avoidance angle can be constructed based on the joint admittance control technology.
- ⁇ q JAL represents the avoidance angle of each joint
- ⁇ JAL represents the virtual repulsion of each joint
- M q , D and K are the virtual inertia matrix, virtual damping matrix and virtual stiffness matrix based on joint admittance control respectively
- s represents Differential operator. It can be understood that the relational expression here is described in the form of a transfer function.
- the above virtual inertia matrix M q is related to the information related to the joint angle of the robot, and can be specifically calculated by the acquired joint angle, joint angular velocity, and the like.
- step S310 "constructing a mapping equation from joint angle-related information of a joint to desired motion state information"
- it can be constructed according to actual requirements.
- mapping equation For example, for force-controlled or force-position hybrid joints, based on the acceleration level, the following mapping equation can be constructed:
- J is the Jacobian matrix; represents the joint angular acceleration to be solved; represents the desired acceleration of the joint in the task space; is the derivative of the Jacobian matrix J; is the feedback joint angular velocity.
- mapping equation can be constructed based on the velocity level:
- J is the Jacobian matrix; represents the joint angular velocity to be solved; Represents the desired velocity of the joint in the task space.
- step S320 "constructing the expression of the joint angular velocity or joint angular acceleration change based on the relevant information of the joint's avoidance angle", as shown in FIG. 4 , including:
- a function expression of the activation factor related to the joint angle is constructed based on the angle safety range of the joint, and the function expression is used to solve the activation factor at the corresponding moment.
- the functional expression of the activation factor can be constructed according to the angle safety range of the joint, wherein the activation factor is related to the angle of the corresponding joint.
- the activation factor when the joint angle is between the upper boundary and the lower boundary of the safety range, the activation factor is 0, and when the joint angle exceeds the safety range boundary and approaches the limit, the activation factor tends to 1 from 0.
- Sub-step S322 a projection matrix reflecting the dynamic change of the joint angle is constructed with the activation factor as the diagonal element.
- the projection matrix ⁇ constructed with the activation factors corresponding to each joint as the elements on the main diagonal, there are: when the i-th diagonal element of ⁇ is zero, it means that the i-th diagonal element is zero.
- the joint moves without limit avoidance, and when the angle of joint i exceeds the upper or lower boundary of the safe range and gradually approaches the limit boundary, the limit avoidance motion of the i-th joint increases gradually, and the i-th diagonal element of ⁇ will gradually become 1.
- the activation factor beta gram can be expressed as follows:
- a projection matrix ⁇ is constructed with the activation factors as diagonal elements, where ⁇ R n ⁇ n . It can be understood that when the ith diagonal element of ⁇ is zero, it means that the ith joint moves without limit avoidance, and when the angle of joint i exceeds the upper or lower boundary of the safety range and gradually approaches the limit boundary, The i-th joint avoidance motion gradually increases, and the i-th diagonal element of ⁇ will gradually become 1.
- Sub-step S323 constructing an expression for the change of joint angular velocity or angular acceleration according to the relevant information of the joint's avoidance angle and the projection matrix.
- an increment related to the joint angular acceleration or joint angular velocity can be defined, and the joint angular velocity or joint angle to be solved can be obtained by using the increment, the joint's avoidance angular acceleration, and the joint angle information actually fed back. acceleration. It can be understood that the increment can reflect the degree of change of the joint angle in the corresponding control period.
- the constructed expression of joint angular acceleration change may be:
- the expression of the convex quadratic programming optimization function can be:
- v 1 represents the equality constraints of the optimization model and/or the first slack variable of the inequality constraints
- C and d are the coefficient matrices of the inequality constraints, which can be determined according to the robot Equality constraints and/or inequality constraints that need to be satisfied during the execution of the task are solved.
- an optimization model at the speed level can be constructed. If the QP optimization function is still used as an example, the expression of the convex quadratic programming optimization function at this time is:
- v 2 represents the second slack variable of the equality constraints and/or inequality constraints.
- v 1 and v 2 are only used to distinguish slack variables in different optimization function expressions, and the slack variable is an optimization variable constructed for the optimization and solution of the OP optimization function.
- the inequality constraints that the robot needs to satisfy during the task execution process may include, but are not limited to, one or more of joint torque constraints, friction cone constraints for contact with the soles of the feet, and gravity ground projection constraints.
- the specific constraints may be based on set according to actual needs.
- the inverse dynamics equation of the robot can be obtained from the inverse dynamics modeling method of the robot, which is usually expressed in the following form:
- M represents the inertia matrix of the robot
- ⁇ c represents the torque generated by the Coriolis force of the robot
- ⁇ g represents the torque generated by the robot's gravity
- J T F represents the torque term generated by the external force F on the robot. Therefore, according to the form of the above constraints, it can be expressed as the following form:
- the increment of the joint angular velocity or joint angular acceleration can be obtained by solving, and then the increment can be substituted into the above-mentioned variation expression, and the required value in the above-mentioned step S30 can be calculated.
- the robot motion control method in this embodiment is described in conjunction with the corresponding embodiments, without planning a collision avoidance trajectory, the robot can generate a reflective collision avoidance behavior in real time, and the robot can run safely and stably from the software algorithm level.
- this embodiment provides a robot motion control apparatus 100 .
- the robot motion control device 100 includes:
- the expected value calculation module 110 is configured to calculate the expected motion state information of each joint of the robot for performing the task in the corresponding control period according to the planned task trajectory information and the joint angle related information fed back by the robot in the corresponding control period, and the joint angle related information includes: each joint angle.
- the limit avoidance angle information calculation module 120 is configured to solve the virtual repulsion force of each joint away from the limit corresponding to the control period based on the feedback of each joint angle, and calculate the limit avoidance angle related information of each joint according to the virtual repulsion force.
- the optimization model solving module 130 is configured to optimally solve the joint smoothing and limiting avoidance optimization model based on the desired motion state information of the corresponding control period and the related information of the limit avoidance angle, so as to obtain the joint smoothness and limit avoidance and use Joint angular acceleration or joint angular velocity for performing the task.
- the control instruction execution module 140 is configured to use the joint angular acceleration or joint angular velocity to generate joint control instructions corresponding to a control period, and the joint control instructions of each control period are used to control the robot to move to complete the planned task.
- the present application also provides a robot, for example, the robot may be a robotic arm, a humanoid robot, a multi-legged robot, etc. used in different fields.
- the robot includes a processor and a memory, wherein the memory stores a computer program, and the processor runs the computer program, so that the robot executes the above-mentioned robot motion control method or each module in the above-mentioned robot motion control device. Function.
- the present application also provides a readable storage medium for storing the computer program used in the above-mentioned robot.
- each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more functions for implementing the specified logical function(s) executable instructions. It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures.
- each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams can be implemented using dedicated hardware-based systems that perform the specified functions or actions. be implemented, or may be implemented in a combination of special purpose hardware and computer instructions.
- each functional module or unit in each embodiment of the present application may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.
- the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium.
- the technical solution of the present application can be embodied in the form of a software product in essence, or the part that contributes to the prior art or the part of the technical solution.
- the computer software product is stored in a storage medium, including Several instructions are used to cause a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application.
- the aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
一种机器人运动控制方法、装置和机器人，该方法包括：根据任务轨迹信息和机器人实时反馈的关节角相关信息计算对应控制周期的期望运动状态信息(S10)；利用各个关节角求解对应控制周期的虚拟斥力并根据虚拟斥力计算避限位角相关信息(S20)；基于对应控制周期的期望运动状态信息和避限位角相关信息对优化模型求解，得到满足关节平滑避限位且用于执行任务的最优关节角加速度或角速度(S30)以用于在对应控制周期控制该机器人运动(S40)。根据每个控制周期的机器人状态，平滑地产生远离关节限位的行为，无需预先规划避撞轨迹，并在自由度满足的情况下，仍能保证规划任务的顺利执行，使得机器人安全稳定地运行。
Description
相关申请的交叉引用
本申请要求于2021年04月02日提交中国专利局的申请号为2021103606600、名称为“机器人运动控制方法、装置和机器人”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
本申请涉及机器人控制技术领域，尤其涉及一种机器人运动控制方法、装置和机器人。
随着机器人技术的发展，机器人正逐渐从结构化的环境中走出来，走进动态多变的非结构化环境中。在机器人执行多种复杂任务(机械臂运动跟随、足式机器人质心动量控制等)以及受到各种外界干扰(躲避障碍物撞击、柔顺响应外力拖拽等)时，均有可能产生关节超限位的问题。为此，在机器人在执行任务过程中，如何保证当关节角度接近关节限位时，能够平稳地远离关节限位，是实现机器人在非结构化环境中安全稳定运行的关键。
申请内容
本申请实施例提供一种机器人运动控制方法、装置和机器人，可以实现平稳地远离关节限位，且在自由度足够的情况下，仍保证原有任务能够顺利执行，从而使机器人安全稳定地运行。
本申请的实施例提供一种机器人运动控制方法，包括：
根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；
基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；
基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑 避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；
利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
在本申请的一些实施例中，根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，包括：
根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息；
获取机器人在对应控制周期反馈的关节角相关信息，并根据所述关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息；
根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
在本申请的一些实施例中，所述虚拟斥力的表达式基于关节的关节角和关节限位角范围预先构建，包括：
根据关节限位角范围确定所述关节的角度安全变化的上边界和下边界，以得到所述关节的角度安全范围；
当实际反馈的关节角位于所述关节的角度安全范围内，则所述虚拟斥力为0；当实际反馈的关节角位于所述关节的角度安全范围外，则在所述关节角越接近所述关节限位角范围的上限或下限的情况下，所述虚拟斥力越大且方向远离关节限位方向。
在本申请的一些实施例中，所述避限位角相关信息包括关节的避限位角，所述关节的避限位角与虚拟斥力的关系式基于关节导纳控制构建，所述关系式如下：
其中，Δq
JAL表示关节的避限位角；τ
JAL表示虚拟斥力；M
q、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。
在本申请的一些实施例中，所述关节平滑避限位优化模型经过预先构建，包括：
构建关节的关节角相关信息到期望运动状态信息的映射方程；
基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式；
基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建平滑避限位优化模型，其中，所述平滑避限位优化模型的优化变量与关节角速度或关节角加速度相关，基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建所述平滑避限位优化模型的约束条件。
在本申请的一些实施例中，所述基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式，包括：
基于关节的所述角度安全范围构建与关节角相关的激活因子的函数表达式，所述函数表达式用于求解所述关节在对应控制周期的激活因子；
以所述激活因子为对角元构建用于反映关节角动态变化的投影矩阵；
根据所述避限位角相关信息和所述投影矩阵构建关节角速度或关节角加速度变化的表达式。
在本申请的一些实施例中，所述避限位角相关信息还包括关节的避限位角加速度，所述期望运动状态信息包括关节的期望加速度；
若该机器人的关节类型为力控型关节或力位混合型关节，则基于所述映射方程和所述关节角加速度变化的表达式的联立方程来构建基于与关节角加速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
其中，J为雅克比矩阵；
表示待求解的关节角加速度；
表示关节在任务空间的期望加速度；
为雅克比矩阵J的导数；
为实际反馈的关节角速度；
以及，所述关节角加速度变化的表达式为：
其中，
表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ
1表示关节角加速度的增量。
在本申请的一些实施例中，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v
1表示所述等式约束和/或不等式约束的第一松弛变量。
在本申请的一些实施例中，所述避限位角相关信息还包括关节的避限位角速度，所述期望运动状态信息包括关节的期望速度；
若该机器人的关节类型为位控型关节，则基于所述映射方程和所述关节角速度变化的表达式的联立方程构建基于与关节角速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
其中，J为雅克比矩阵；
表示待求解的关节角速度；
表示关节在任务空间的期望速度；
以及，所述关节角速度变化的表达式为：
其中，
表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ
2表示关节的角速度的增量。
在本申请的一些实施例中，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v
2表示所述等式约束和/或不等式约束的第二松弛变量。
本申请实施例还提供一种机器人运动控制装置，包括：
期望值计算模块，用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；
避限位角信息计算模块，用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；
优化模型求解模块，用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；
控制指令执行模块，用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
本申请的实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机器人运动控制方法。
本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时，实施上述的机器人运动控制方法。
本申请的实施例具有如下有益效果：
本申请的实施例的机器人运动控制方法通过构建关节在远离限位方向上产生的虚拟斥力并结合关节导纳控制来求解避限位角相关信息，还通过构建平滑避限位的优化问题，优化求解得到能关节平滑避限位的各个关节的控制指令来控制机器人执行规划任务，可以实现机器人平滑地产生远离关节限位的行为，并在自由度满足的情况下，仍能保证原规划任务的顺利执行，使得机器人安全稳定地运行。该方法无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，使得从软件算法层面保证机器人安全稳定运行等。
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本申请实施例的机器人运动控制方法的流程示意图；
图2示出了本申请实施例的机器人运动控制方法的关节平滑避限位优化模型构建的流程示意图；
图3示出了本申请实施例的机器人运动控制方法的期望运动状态信息获取的流程示意图；
图4示出了本申请实施例的机器人运动控制方法的关节角速度或关节角加速度变化的表达式构建的流程示意图；
图5示出了本申请实施例的机器人运动控制装置的结构示意图。
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
对于机器人如何远离关节限位的问题，现有的方法主要有两类：(1)通过运动规划的方法，预先规划出使机器人远离关节限位的运动轨迹，然后将所得各关节运动轨迹的指令发送给驱动器从而控制各关节电机实现远离关节限位的运动。(2)通过逆运动学，优化求解任务空间期望运动下的各关节运动，当关节空间的期望转角即将与关节限位发生碰撞时，对运动进行截断。
但现有方法存在以下不足：对于第一种运动规划的方法，当机器人自由度较多、外界干扰(例如未来一段时间内障碍物的撞击轨迹或外力拖拽等扰动的大小和方向)难以预测时，这种运动规划的方法将难以实时在线地产生新的避限位轨迹，此时难以采用此类方法进行避限位。而对于第二种运动截断的方法，虽然能够防止碰撞的发生，但会造成机器人各关节加速度、速度和位置指令的突变，影响机器人运行平稳性和任务执行效果，甚至会造成机器人剧烈振动或急停等现象。
对此，本申请实施例提出一种机器人运动控制方法，该方法无需预先规划避撞轨迹，而能够平滑地产生远离关节限位的行为，并在自由度满足的情况下，优化求解得到的各 关节控制指令，仍能保证任务的顺利执行，使得机器人安全稳定地运行。下面结合具体的实施例来对该方法进行说明。
实施例1
请参照图1，本实施例提出一种机器人运动控制方法，可适用于具有位控型、力控型关节以及力位混合控制型等关节类型的机器人，该方法是一种通用的运动控制方法，适用于工业、服务和特种等多种不同领域的机械臂、仿人机器人、多足机器人等的运动控制。
如图1所示，下面对该机器人运动控制方法进行详细说明。
步骤S10，根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算该机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
其中，上述的任务轨迹通常由预先规划得到，作为机器人执行期望任务时的在任务空间的运动参考轨迹，该任务轨迹可以指包含多个控制周期在内的一段时间的参考轨迹，也可以是对应控制周期的参考轨迹，在此并不作限定。例如，若获得的是一段时间的参考轨迹信息，则在计算每个控制周期的期望运动状态信息时，可从中取出对应控制周期的参考信息进行计算。示范性地，该任务轨迹信息可包括但不限于为机器人执行规划任务的参考位置、参考速度以及参考加速度等。
上述，关节角相关信息主要是指该机器人在每个控制周期内实时反馈在关节空间的实际关节角、关节角速度和/或关节角加速度等信息。例如，可通过如角度编码器等传感器来采集得到。当然，除了实时反馈的关节角相关信息外，机器人还可根据需求来获得其他的状态反馈信息，在此并不作限定。例如，可通过安装在机械臂末端处的力传感器等力传感器或力矩传感器等来测量机器人末端实际受到的外力等，又或者，可通过如惯性测量单元(IMU)等来测量得到机器人的躯干姿态等。
对于上述步骤S10，示范性地，利用机器人实时反馈的实际状态信息和规划的参考值，结合反馈控制技术可计算获得机器人各个关节在任务空间的期望值。值得注意的是，该实时反馈得到的实际状态信息为机器人在关节空间的相关信息，在与参考值作运算前，需要将其转换为在任务空间对应的信息。通过考虑任务空间中机器人的实际状态与规划参考值之间的偏差，可以保证机器人在任务执行时的精度。
步骤S20，基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息。
其中，该虚拟斥力是实际的关节角度在靠近关节限位角时定义的一种虚拟的用于排斥或远离关节限位的力或力矩，其方向为远离关节限位的方向。可以理解，该虚拟斥力可以指虚拟斥力，也可以指虚拟斥力矩，例如，对于平动型关节，由于无法产生力矩，则构建的是虚拟斥力；而对于转动型关节，由于可产生转动力矩，此时构建的为虚拟斥力矩。
例如，可通过预先构建与关节角、关节限位角度范围有关的虚拟斥力。其中，所述的关节限位角度范围是指该关节能够达到的最大角度范围，由角度上限和角度下限构成该限位角度范围，其大小往往取决于机器人实际机械结构设计。另外，考虑到关节角在到达限位角度的上限或下限之前通常有一段安全的转角范围，此时通常不会产生虚拟斥力，故还设定了关节的角度安全范围，即在该角度安全范围内关节可以任意转角而不会出现障碍。
示范性地，在构建该虚拟斥力时，应当满足：当实际反馈的关节角位于关节的角度安全范围内，则该虚拟斥力即为0；当实际反馈的关节角位于关节的角度安全范围外，则在所述关节角越接近关节限位角范围的上限或下限的情况下，此时产生的虚拟斥力越大且方向为远离关节限位方向。
其中，上述的避限位角相关信息可包括关节的避限位角、避限位角速度和/或避限位角加速度等。所述的避限位角是指远离关节限位的关节角，同理，避限位角速度和避限位角加速度分别是指远离关节限位的关节角速度和远离关节限位的关节角加速度。
为求解出关节的避限位角相关信息，可预先构建出虚拟斥力与避限位角度之间的关系式，在一种实施方式中，可基于关节导纳控制技术等来构建该虚拟斥力与关节角、关节的避限位角度之间的关系式，当然还可以采用其他方式构建，在此并不限定。通常地，由于关节角越接近关节限位时，虚拟斥力越大，进而计算得到的避限位角速度和避限位角加速度也将越大。
对于上述步骤S20，示范性地，获取到对应控制周期实时反馈的各个关节角后，将其代入虚拟斥力的表达式即可计算得到该对应控制周期内各个关节的虚拟斥力，进而，利用虚拟斥力与避限位角度之间的关系式来计算出对应控制周期内各个关节的避限位角相关信息，如避限位角速度和避限位角加速度等。通过构建虚拟斥力来进一步求解各个关节在远离关节限位且满足任务需求的关节角速度和/或关节角加速度，从而实现运动过程中的平滑避限位功能。
步骤S30，基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度。
其中，对于上述的关节平滑避限位优化模型，本实施例将通过预先构建一个关节平滑避限位优化模型，即构建一个平滑避限位优化问题，该优化模型采用与关节角加速度或关节角速度有关的参量作为优化变量，例如，该关节角加速度或关节角速度的增量(也可称调整量或变化增量)等。
进而，通过对该优化模型进行最优求解，并基于最优解得到最优关节角速度或关节角加速度，基于该最优关节角速度或关节角加速度得到的关节控制指令，既满足平滑地产生远离关节限位，又在自由度满足的情况下，可以保证机器人顺利执行规划的任务。
示范性地，如图2所示，该平滑避限位优化模型的预先构建，包括：
步骤S310，构建关节的关节角相关信息到期望运动状态信息的映射方程。
示范性地，根据机器人关节与末端速度之间的雅克比矩阵，可构建出实际反馈的关节角相关信息和任务空间的期望运动状态信息的映射方程。例如，对于力控型或力位混合型等关节，可构建基于加速度层面的映射方程；而对于力控型关节，由于不需利用加速度计算关节力矩，可构建出基于速度层面的映射方程。
步骤S320，基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式。
示范性地，可预先构建关节角速度或关节角加速度与避限位角信息之间的关节式，进而方便求解各个控制周期的关节角速度或关节角加速度变化。而为更好地体现机器人在执行任务过程中的自由度变化情况，本实施例将通过构建一个投影矩阵来反映关节自由度的变化情况，而该投影矩阵将用于优化模型的构建。其中，对于该投影矩阵的构建，例如，可基于关节的角度安全范围构建与关节角度相关的激活因子，进而利用该激活因子作为对角元来构建该投影矩阵。上述的步骤S310和S320并不限定执行的顺序，两者可以是同时执行，也可以按照相应的先后顺序执行。
步骤S330，基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建基于与关节角速度或关节角加速度有关的参量作为优化变量的关节平滑避限位优化模型。
示范性地，将映射方程和关节角速度或角加速度变化的表达式进行联立，于是可得 到两者的联立方程。进而，根据联立方程可构建出一个关于实现关节平滑避限位的优化问题。例如，该平滑避限位优化模型将以该关节角速度或关节角加速度的增量作为优化变量，当然，也可以有其他的变量，在此并不限定。
此外，为使机器人能够顺利执行规划的任务，还基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建该关节平滑避限位的优化问题的约束条件。可知，通过对该优化模型进行最优求解决，进而可计算得到最优的关节角速度或关节角加速度。
在一种实施方式中，对于该关节平滑避限位的优化模型，例如，可采用如凸二次规划(quadratic programming，QP)优化函数等进行构建，当然还可以采用其他的优化函数或基于该QP优化函数作适应性调整而构建的优化函数等来构建，都应当在本申请的保护范围之内。
应当明白的是，可根据不同类型的关节来确定是构建基于加速度层面的优化模型，还是构建基于速度层面的优化模型。对于加速度层面，则可选取关节角加速度有关的参量作为优化变量，对于速度层面，则可选取关节角速度有关的参量作为优化变量。
可以理解，通过上述的平滑避限位优化模型可以求解出最优的关节的角速度的增量，进而将其反代回相应的关节角速度或关节角加速度变化的表达式中，即可计算出最优的关节角加速度或关节角速度。由于上述的优化模型包含了投影矩阵，即考虑了关节的自由度变化情况，可以得知，当某个自由度方向未被占用，所需执行的任务不会受到避限位运动的影响。而当该方向自由度逐渐被占用，后续的任务逐渐则会利用剩余的自由度(未超过关节限位)来尽可能执行，进而优先保证了平滑的关节避限位功能，同时在自由度足够的情况下，仍能有效保证所需执行任务依靠剩余处于安全范围内的关节来完成。
步骤S40，利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的关节控制指令用于控制该机器人运动以完成规划的任务。
示范性地，在计算出最优的关节角加速度时，可通过数值积分可得关节角速度指令和关节角度指令；再者，结合动力学模型即可计算得到关节力矩指令。若得到的为最优的关节角速度，则通过数值积分即可得关节的位置指令。进而，将计算出相应的关节控制指令发送到对应的关节，以控制该机器人执行当前任务。可以理解，若当前控制周期执行完指令后，将进入下一控制周期，每个控制周期都将重复上述的步骤S10～S40，直到机器人完成所需执行的任务。
可以理解，上述的机器人运动控制方法通过构建虚拟斥力并结合关节导纳控制，能 够使机器人在执行原有任务过程中，即使难以预测未来一段时间内外界的干扰，仍可根据当前控制周期机器人的状态，平滑地产生远离关节限位的行为，即实现平滑避限。而通过构建平滑避限位的优化问题，使接近限位的关节自由度逐渐被避限位运动占用，而机器人所需执行的任务逐渐变为在剩余的未超限位关节所构成的自由度中求解，从而实现了在关节超限位时优先保证关节避限位，并利用剩余自由度尽可能保证原有任务的顺利执行，而关节未超限位时则利用全部自由度优先执行所需任务。
本实施例的机器人运动控制方法无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，从软件算法层面保证机器人安全稳定运行。此外，该方法输出关节端力矩、速度和位置指令，适用于位控、力控关节以及力位混合控制关节的串/并联构型机器人，适用于工业、服务和特种等多种领域的机械臂，四足机器人，仿人机器人的运动控制，是一种通用的运动控制方法。
实施例2
请参照图3，基于上述实施例1，本实施例提出一种机器人运动控制方法，该方法同样包括上述步骤S10～S40。下面对这些步骤进行具体描述。
在一种实施方式中，对于上述步骤S10“根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息”，如图3所示，包括：
步骤S110，根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息。
针对需要执行的任务，可预先规划出机器人在任务空间的运动轨迹信息，例如，该运动轨迹可以是机械臂末端的运动轨迹、机器人质心的轨迹，也可以是仿人机器人上半身姿态的转动轨迹等，具体可根据实际执行的任务来确定。示范性地，该参考运动状态信息可包括机器人在对应控制周期的在任务空间的参考位置、参考速度以及参考加速度等中的一种或多种。
步骤S120，获取该机器人在对应控制周期反馈的关节角相关信息，并根据该关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息。
本实施例中，实时反馈的关节角相关信息主要包括各个关节的角度(即关节角)、角速度(即关节角速度)及角加速度(即关节角速度)。例如，可通过安装在各关节处 的角度编码器等采集对应关节的角度，进而对关节角度进行微分处理，可计算得到对应的关节角速度及关节角加速度等。
可以理解，通过传感器测量得到的是关节空间的角度相关信息，故可通过正向运动学计算出机器人在任务空间对应的实际运动状态信息，该实际运动状态信息主要包括实际位置、实际速度和实际加速度等信息。值得注意的是，上述的步骤S110和S120并不严格限定执行的顺序，两者可以是同时执行，也可以按照相应的先后顺序执行。
步骤S130，根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
示范性地，上述预设的关节控制律也称反馈控制律，例如，可以是机械臂出厂时设置或预先生成的，也可以是由用户根据实际需求预先设置的，其预设形式并不作限定。在一种实施方式中，该预设的关节控制律满足：
其中，x
act和
分别表示实际位置和实际速度；x
ref、
和
分别表示参考位置、参考速度和参考加速度；K
p和K
d分别为该关节控制律中的位置增益和速度增益；
表示期望加速度。进一步地，可通过对该期望加速度进行积分以得到期望速度
和期望位置
可以理解，该关节控制律仅仅是一种示例，在实际运用中也可以根据实际需求对该关节控制律进行相应调整。
于是，在获得对应控制周期内的实际运动状态信息和参考运动状态信息后，基于预设的关节控制律即可求解出对应控制周期的期望运动状态信息。例如，当前的期望运动状态信息可包括期望位置、期望速度和/或期望加速度等。
在本申请的一些其他的实施例中，对于上述步骤S20中的“基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力”，其中，关于该关节的角度安全范围和虚拟斥力的求解表达式，在一种实施方式中，该关节的角度安全范围的上边界和下边界可根据关节限位角范围来进一步确定。
示范性地，关节的角度安全范围与关节限位角范围满足如下关系：
其中，
和
分别表示第i个关节的角度安全范围的上边界和下边界；
和
分别表示第i个关节的限位角范围的上限和下限；比例系数α可根据工程经验来相应设定。当然，也可以采用其他的方式来选取出角度安全范围，在此并不作限定。
进而，根据上述步骤S20中描述的虚拟斥力的构建原则，在一种实施方式中，基于上述的关节限位角度范围及角度安全范围的关系式，若该机器人所有关节的虚拟斥力记为τ
JAL，则有：
其中，
其中，
表示第i个关节的虚拟斥力矩；τ
max为最大关节力矩。可以理解，这里的第i个关节仅是以转动型关节进行描述。若为平动型关节，将该最大关节力矩τ
max替代为最大关节力f
max即可。
由上述表达式可知，当关节i的角度q
i位于关节角度安全范围的上边界
和下边界
之间时，该关节的虚拟斥力为0，此时无需进行关节避限位。而当关节角度q
i大于
或小于
时，会产生使关节角度远离限位方向的虚拟斥力，即关节角度越接近限位方向，则虚拟斥力越大。当关节角度q
i完全达到限位
或
时，则虚拟斥力最大。
在本申请的一些其他实施例中，对于上述步骤S20中的“根据所述虚拟斥力计算各个关节的避限位角相关信息”，其中，关于虚拟斥力与关节角、关节的避限位角度之间的关系式的预先构建，例如，可基于关节导纳控制技术来构建该虚拟斥力与关节角、关 节的避限位角度之间的关系式。
示范性地，该关系式如下：
其中，Δq
JAL表示各个关节的避限位角；τ
JAL表示各个关节的虚拟斥力；M
q、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。可以理解，这里的关系式通过传递函数的形式描述。其中，上述的虚拟惯量矩阵M
q与机器人的关节角相关信息有关，具体可通过获取的关节角、关节角速度等计算得到。
由上述关系式可知，关节角越接近限位，则虚拟斥力越大，进而计算所得的避限位角速度
和避限位角加速度
也越大。
在本申请的一些其他实施例中，对于上述步骤S310“构建关节的关节角相关信息到期望运动状态信息的映射方程”，可根据实际需求来构建。
例如，对于力控型或力位混合型等关节，基于加速度层面，可构建出如下映射方程：
其中，J为雅克比矩阵；
表示待求解的关节角加速度；
表示关节在任务空间的期望加速度；
为雅克比矩阵J的导数；
为反馈的关节角速度。
又例如，对于力控型关节，由于不需利用加速度计算关节力矩，基于速度层面，可构建出如下映射方程：
其中，J为雅克比矩阵；
表示待求解的关节角速度；
表示关节在任务空间的期望速度。
进一步地，对于上述步骤S320“基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式”，如图4所示，包括：
子步骤S321，基于关节的角度安全范围构建与关节角度相关的激活因子的函数表达式，所述函数表达式用于求解对应时刻的激活因子。
示范性地，可根据关节的角度安全范围来构建激活因子的函数表达式，其中，该激活因子与对应关节的角度有关。示范性地，当关节角度在安全范围上边界和下边界之间时，激活因子为0，而当关节角度超过安全范围边界接近限位时，激活因子由0趋于1。
子步骤S322，以激活因子为对角元构建反映关节角度动态变化的投影矩阵。
示范性地，对于以各个关节对应的激活因子作为主对角线上的元素而构建得到的投影矩阵Λ，则有：当Λ的第i个对角元为零时，此时表示第i个关节无避限位运动，而当关节i角度超过安全范围的上边界或下边界并逐渐接近限位边界时，第i个关节避限位运动逐渐增大，且Λ的第i个对角元会逐渐变为1。
例如，在一种实施方式中，激活因子β
i克可表示为如下形式：
其中，f
β(x)为以x为自变量的从0到1的平滑函数，x＝0时，f
β(x)＝0，
时，f
β(x)＝1，该平滑函数可通过现有多项式、样条曲线等方式拟合得到。
于是，将激活因子作为对角元来构建一投影矩阵Λ，其中，Λ∈R
n×n。可以理解，Λ的第i个对角元为零时，此时表示第i个关节无避限位运动，而当关节i角度超过安全范围的上边界或下边界并逐渐接近限位边界时，第i个关节避限位运动逐渐增大，且Λ的第i个对角元会逐渐变为1。
子步骤S323，根据关节的避限位角相关信息和所述投影矩阵构建关节角速度或角加速度变化的表达式。
示范性地，可通过定义一个与关节角加速度或关节角速度有关的增量，利用该增量、关节的避限位角加速度及实际反馈的关节角信息等来得到待求解的关节角速度或关节角加速度。可以理解，该增量能够反映出关节角在对应控制周期的变化程度。
在一实施方式中，示范性地，构建的关节角加速度变化的表达式可为：
其中，
为待求解的关节角加速度；
表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ
1表示关节角加速度的增量。
同样，可构建得到关节角速度变化的表达式，如下：
其中，
为待求解的关节角速度；
表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ
2表示关节角速度的增量。
于是，将该关节角加速度或关节角速度变化的表达式，结合上述步骤S310所构建得到的映射方程进行联立，可得到如下的联立方程：
或者，
进一步可选地，对于上述步骤S330中构建的平滑避限位优化模型，若以QP优化函数来构建加速度层面的优化模型，示范性地，该凸二次规划优化函数的表达式可为：
其中，
表示机器人执行任务过程中需满足的不等式约束的通用形式，v
1表示优化模型的等式约束和/或不等式约束的第一松弛变量；C和d均为不等式约束的系数矩阵，可根据该机器人执行任务过程中需满足的等式约束和/或不等式约束求解。
又或者，可以构建速度层面的优化模型，若仍以QP优化函数为例，此时的凸二次规划优化函数的表达式为：
其中，v
2表示所述等式约束和/或不等式约束的第二松弛变量。这里的v
1和v
2仅是为了区分为不同优化函数表达式中的松弛变量，该松弛变量是为了对OP优化函数进行最优化求解而构建的一优化变量。
可以理解，对于不同关节类型的机器人，由于所需的关节控制指令的不同，可构建出不同的关节平滑避限位优化模型。例如，对于力控型关节或力位混合型关节，可构建 基于加速度层面的关节平滑避限位的优化问题。而对于位控型关节，由于无需通过动力学模型计算关节力矩，则可构建基于速度层面的关节平滑避限位的优化问题。
应当明白的是，对于上述构建的平滑避限位优化模型，为保证其求解得到的结果能够保证机器人能执行任务或避限位，还需要为该优化模型设置一些约束条件。例如，该机器人执行任务过程中需满足的不等式约束可包括但不限于包括，关节力矩约束、足底接触的摩擦锥约束和重力地面投影约束等中一种或多种，具体的约束条件可根据实际需求来设定。
以关节力矩约束为例，当要求机器人满足关节力矩约束时，即驱动关节力矩不能超过关节的最大力矩，由机器人的逆动力学建模方法可得到机器人逆动力学方程，通常表示为如下形式：
其中，M表示机器人的惯量矩阵；τ
c表示机器人的科氏力产生的力矩，τ
g表示机器人重力产生的力矩，J
TF表示机器人所受外力F产生的力矩项。于是，按照上述的约束条件的形式，可以表示为如下形式：
其中，可得到C表示为
而d表示为
可以理解，通过求解上述的优化模型，可以求解得到关节角速度或关节角加速度的增量，进而将该增量回代到上述的变化表达式中，即可计算出上述步骤S30中的满足需求的关节角速度
或关节角加速度
以加速度层面的优化模型的求解过程进行分析，可知，当关节i的角度处于角度安全范围时，
Λ的第i个对角元为零，此时第i个关节无避限位运动，I-Λ矩阵的第i个对角元为1，因此该自由度方向未被占用，所需执行的任务不会受到关节i避限位运动的影响。而当关节i的角度超过安全范围的边界并逐渐接近角度限位的上下限时，第i个关节的避限位角加速度
将逐渐增大，且Λ的第i个对角元会逐渐变为1，I-Λ矩阵的第i个对角元趋于0，该方向自由度逐渐被占用，使得i关节加速度求解结果趋近于避限位加速度
后续任务逐渐则利用剩余自由度尽可能满足任 务执行精度，进而优先保证了平滑的关节避限位功能，同时在自由度足够的情况下，仍能有效保证所需执行任务依靠剩余处于安全范围内的关节来完成。而当关节i的角度远离限位时，Λ的第i个对角元会逐渐变为0，I-Λ矩阵第i个对角元趋于1，该方向自由度逐渐被释放，使得机器人逐渐获得更多自由度优先执行所需任务。
本实施例的机器人运动控制方法通过结合相应的实施例来说明，无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，从软件算法层面保证机器人安全稳定运行。
实施例3
请参照图5，基于上述实施例1的方法，本实施例提出一种机器人运动控制装置100。示范性地，该机器人运动控制装置100包括：
期望值计算模块110用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角。
避限位角信息计算模块120用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息。
优化模型求解模块130用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度。
控制指令执行模块140用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。
本申请还提供了一种机器人，例如，该机器人可以是运用于不同领域的机械臂、仿人机器人、多足机器人等。示范性地，该机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的机器人运动控制方法或者上述机器人运动控制装置中的各个模块的功能。
本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过 其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。
Claims (13)
- 一种机器人运动控制方法，其特征在于，包括：根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
- 根据权利要求1所述的机器人运动控制方法，其特征在于，所述根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，包括：根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息；获取机器人在对应控制周期反馈的关节角相关信息，并根据所述关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息；根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
- 根据权利要求1所述的机器人运动控制方法，其特征在于，所述虚拟斥力的表达式基于关节的关节角和关节限位角范围预先构建，包括：根据关节限位角范围确定所述关节的角度安全变化的上边界和下边界，以得到所述关节的角度安全范围；当实际反馈的关节角位于所述关节的角度安全范围内，则所述虚拟斥力为0；当实际反馈的关节角位于所述关节的角度安全范围外，则在所述关节角越接近所述关节限位角范围的上限或下限的情况下，所述虚拟斥力越大且方向远离关节限位方向。
- 根据权利要求1至3中任一项所述的机器人运动控制方法，其特征在于，所述避 限位角相关信息包括关节的避限位角，所述关节的避限位角与虚拟斥力的关系式基于关节导纳控制构建，所述关系式如下：
其中，Δq JAL表示关节的避限位角；τ JAL表示虚拟斥力；M q、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。 - 根据权利要求3所述的机器人运动控制方法，其特征在于，所述关节平滑避限位优化模型经过预先构建，包括：构建关节的关节角相关信息到期望运动状态信息的映射方程；基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式；基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建平滑避限位优化模型，其中，所述平滑避限位优化模型的优化变量与关节角速度或关节角加速度相关，基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建所述平滑避限位优化模型的约束条件。
- 根据权利要求5所述的机器人运动控制方法，其特征在于，所述基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式，包括：基于关节的所述角度安全范围构建与关节角相关的激活因子的函数表达式，所述函数表达式用于求解所述关节在对应控制周期的激活因子；以所述激活因子为对角元构建用于反映关节角动态变化的投影矩阵；根据所述避限位角相关信息和所述投影矩阵构建关节角速度或关节角加速度变化的表达式。
- 根据权利要求6所述的机器人运动控制方法，其特征在于，所述避限位角相关信息还包括关节的避限位角加速度，所述期望运动状态信息包括关节的期望加速度；若该机器人的关节类型为力控型关节或力位混合型关节，则基于所述映射方程和所述关节角加速度变化的表达式的联立方程来构建基于与关节角加速度有关的优化变量的平滑避限位优化模型；所述映射方程为：
其中，J为雅克比矩阵；表示待求解的关节角加速度；
表示关节在任务空间的期望加速度；
为雅克比矩阵J的导数；
为实际反馈的关节角速度；
以及，所述关节角加速度变化的表达式为：
其中，表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ 1表示关节角加速度的增量。
- 根据权利要求7所述的机器人运动控制方法，其特征在于，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v 1表示所述等式约束和/或不等式约束的第一松弛变量。 - 根据权利要求6所述的机器人运动控制方法，其特征在于，所述避限位角相关信息还包括关节的避限位角速度，所述期望运动状态信息包括关节的期望速度；若该机器人的关节类型为位控型关节，则基于所述映射方程和所述关节角速度变化的表达式的联立方程构建基于与关节角速度有关的优化变量的平滑避限位优化模型；所述映射方程为：
其中，J为雅克比矩阵；表示待求解的关节角速度；
表示关节在任务空间的期望速度；
以及，所述关节角速度变化的表达式为：
其中，表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ 2表示关节的角速度的增量。
- 根据权利要求9所述的机器人运动控制方法，其特征在于，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v 2表示所述等式约束和/或不等式约束的第二松弛变量。 - 一种机器人运动控制装置，其特征在于，包括：期望值计算模块，用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；避限位角信息计算模块，用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；优化模型求解模块，用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；控制指令执行模块，用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述任务。
- 一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-10中任一项所述的机器人运动控制方法。
- 一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时，实施根据权利要求1-10中任一项所述的机器人运动控制方法。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110360660.0 | 2021-04-02 | ||
| CN202110360660.0A CN113070881B (zh) | 2021-04-02 | 2021-04-02 | 机器人运动控制方法、装置和机器人 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2022205940A1 true WO2022205940A1 (zh) | 2022-10-06 |
Family
ID=76614915
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2021/131890 WO2022205940A1 (zh) | 2021-04-02 | 2021-11-19 | 机器人运动控制方法、装置和机器人 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN113070881B (zh) |
| WO (1) | WO2022205940A1 (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116700012A (zh) * | 2023-07-19 | 2023-09-05 | 合肥工业大学 | 一种多智能体的避撞编队合围控制器的设计方法 |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113070881B (zh) * | 2021-04-02 | 2022-11-11 | 深圳市优必选科技股份有限公司 | 机器人运动控制方法、装置和机器人 |
| CN113618742B (zh) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | 一种机器人避障方法、装置和机器人 |
| CN113618741B (zh) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | 机器人关节位姿优化方法、机器人控制方法和机器人 |
| CN113618739B (zh) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | 一种机器人动态避障方法、装置和机器人 |
| CN114084295A (zh) * | 2021-11-12 | 2022-02-25 | 上海新纪元机器人有限公司 | 主动减少晃动的方法、系统及座椅 |
| CN113977585B (zh) * | 2021-11-25 | 2024-03-15 | 中国北方车辆研究所 | 一种虚拟力伺服柔顺控制方法 |
| CN114347020B (zh) * | 2021-12-22 | 2024-02-13 | 深圳市优必选科技股份有限公司 | 一种运动控制方法、运动控制装置及机器人 |
| CN114326769B (zh) * | 2021-12-28 | 2024-03-29 | 深圳市优必选科技股份有限公司 | 机器人运动矫正方法及装置、机器人控制设备和存储介质 |
| CN115229772B (zh) * | 2022-08-23 | 2023-07-18 | 深圳市越疆科技股份有限公司 | 机器人及其控制方法、装置、设备、存储介质、机械臂 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130184868A1 (en) * | 2012-01-17 | 2013-07-18 | Seiko Epson Corporation | Robot controller, robot system, robot control method |
| US20180207813A1 (en) * | 2015-08-04 | 2018-07-26 | Suzhou Amtf Robots Co., Ltd | Method for external force to drive moving piece in mechanical joint |
| CN111469129A (zh) * | 2020-04-24 | 2020-07-31 | 苏州新传品智能科技有限公司 | 一种基于双机械臂的船体外板曲面成形无碰撞运动规划方法 |
| CN111538949A (zh) * | 2020-07-10 | 2020-08-14 | 深圳市优必选科技股份有限公司 | 冗余机器人逆运动学求解方法、装置和冗余机器人 |
| CN113070881A (zh) * | 2021-04-02 | 2021-07-06 | 深圳市优必选科技股份有限公司 | 机器人运动控制方法、装置和机器人 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6140612A (ja) * | 1984-08-01 | 1986-02-26 | Shinko Electric Co Ltd | 産業用ロボツト |
| KR102030141B1 (ko) * | 2014-06-11 | 2019-10-10 | 현대자동차(주) | 로봇의 팔꿈치 제어시스템 및 그 제어방법 |
| CN105538327A (zh) * | 2016-03-03 | 2016-05-04 | 吉首大学 | 一种基于突加度的冗余度机械臂重复运动规划方法 |
| CN105922265B (zh) * | 2016-06-20 | 2018-08-24 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
| CN107186711B (zh) * | 2017-05-12 | 2021-06-15 | 广州视源电子科技股份有限公司 | 机械臂的限位保护方法、装置及机器人 |
| CN109108965B (zh) * | 2018-07-27 | 2021-07-23 | 武汉精锋微控科技有限公司 | 一种应用于机械手臂的笛卡尔空间运动预测方法 |
-
2021
- 2021-04-02 CN CN202110360660.0A patent/CN113070881B/zh active Active
- 2021-11-19 WO PCT/CN2021/131890 patent/WO2022205940A1/zh active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130184868A1 (en) * | 2012-01-17 | 2013-07-18 | Seiko Epson Corporation | Robot controller, robot system, robot control method |
| US20180207813A1 (en) * | 2015-08-04 | 2018-07-26 | Suzhou Amtf Robots Co., Ltd | Method for external force to drive moving piece in mechanical joint |
| CN111469129A (zh) * | 2020-04-24 | 2020-07-31 | 苏州新传品智能科技有限公司 | 一种基于双机械臂的船体外板曲面成形无碰撞运动规划方法 |
| CN111538949A (zh) * | 2020-07-10 | 2020-08-14 | 深圳市优必选科技股份有限公司 | 冗余机器人逆运动学求解方法、装置和冗余机器人 |
| CN113070881A (zh) * | 2021-04-02 | 2021-07-06 | 深圳市优必选科技股份有限公司 | 机器人运动控制方法、装置和机器人 |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116700012A (zh) * | 2023-07-19 | 2023-09-05 | 合肥工业大学 | 一种多智能体的避撞编队合围控制器的设计方法 |
| CN116700012B (zh) * | 2023-07-19 | 2024-03-01 | 合肥工业大学 | 一种多智能体的避撞编队合围控制器的设计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113070881A (zh) | 2021-07-06 |
| CN113070881B (zh) | 2022-11-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022205940A1 (zh) | 机器人运动控制方法、装置和机器人 | |
| WO2022166328A1 (zh) | 任务执行控制方法、装置、控制设备及可读存储介质 | |
| Carpentier et al. | A versatile and efficient pattern generator for generalized legged locomotion | |
| US10391632B2 (en) | Generating a robot control policy from demonstrations collected via kinesthetic teaching of a robot | |
| Ren et al. | Learning-based variable compliance control for robotic assembly | |
| CN113681543B (zh) | 一种基于模型预测的机械臂零力控制方法 | |
| JP7324932B2 (ja) | 動的計画コントローラ | |
| WO2011042293A1 (en) | A method for reducing the energy consumption of an industrial robot and an industrial robot system | |
| Omran et al. | Optimal task space control design of a Stewart manipulator for aircraft stall recovery | |
| EP2485875A1 (en) | A method for reducing the energy consumption of an industrial robot and an industrial robot system | |
| Kobilarov | Discrete optimal control on lie groups and applications to robotic vehicles | |
| Bagheri et al. | Experimental and novel analytical trajectory optimization of a 7-dof baxter robot: Global design sensitivity and step size analyses | |
| CN112847373B (zh) | 一种机器人轨迹同步控制方法、计算机可读存储介质 | |
| Goher | A two-wheeled machine with a handling mechanism in two different directions | |
| Gerlagh et al. | Energy-aware adaptive impedance control using offline task-based optimization | |
| Kastritsi et al. | A pHRI Framework for Modifying a Robot's Kinematic Behaviour via Varying Stiffness and Dynamical System Synchronization | |
| CN116736748A (zh) | 构建机器人的控制器的方法和机器人 | |
| Xu et al. | Passive bilateral teleoperation of a car-like mobile robot | |
| Rehberg et al. | Comparison of Optimization-Based Methods for Energy-Optimal Quadrotor Motion Planning | |
| US20230191600A1 (en) | Motion control method, robot, and computer-readable storage medium | |
| WO2023165192A1 (zh) | 机器人控制方法、装置、机器人以及计算机可读存储介质 | |
| WO2023165177A1 (zh) | 构建机器人的控制器的方法、机器人的运动控制方法、装置以及机器人 | |
| US20230311320A1 (en) | Mobile manipulation control method and system of quadruped robot with operation arm | |
| Lotfi et al. | A globally convergent observer for velocity estimation in robotic manipulators with uncertain dynamics | |
| Huang et al. | Research on robust fuzzy logic sliding mode control of Two-DOF intelligent underwater manipulators |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 21934576 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 21934576 Country of ref document: EP Kind code of ref document: A1 |