CN117283545B

CN117283545B - Man-machine loop kinematics parameter self-calibration method and system of power artificial limb

Info

Publication number: CN117283545B
Application number: CN202311225087.8A
Authority: CN
Inventors: 付成龙; 郭艺璇; 冷雨泉; 杜豪; 尹树丛; 马腾; 王毓萱; 黄滨鑫
Original assignee: Southern University of Science and Technology
Current assignee: Southern University of Science and Technology
Priority date: 2023-09-21
Filing date: 2023-09-21
Publication date: 2024-07-16
Anticipated expiration: 2043-09-21
Also published as: CN117283545A

Abstract

The application provides a man-machine loop kinematics parameter self-calibration method and a system of a power artificial limb, wherein the power artificial limb is used for being worn below a hip joint of a user, a camera device is fixed on the power artificial limb and used for acquiring topographic information of surrounding environment, and the method comprises the following steps: by constructing a space dimension chain with self-sampling characteristics of a camera sensor, a geometric constraint measurement space of man-machine rigid-flexible coupling is constructed, so that pose errors caused by arrangement of sampling points of an external instrument are avoided, and kinematic parameters are obtained by designing a closed loop-searching step-by-step identification algorithm, so that accurate unification of human body-artificial limb-environment space references is realized. In addition, the method accurately transmits motion information of the artificial limb, the amputee and the environment, conforms to the trend of human-machine-environment fusion, and lays a good foundation for planning motion control of the power thigh artificial limb under complex terrain.

Description

Man-machine loop kinematics parameter self-calibration method and system of power artificial limb

Technical Field

The application belongs to the field of robot control, and particularly relates to a method and a system for self-calibrating man-machine loop kinematics parameters of a power artificial limb.

Background

With the development of robotics in recent years, powered thigh prostheses with robotic features have emerged. The power thigh artificial limb can reduce the energy consumption required by the walking of the amputee through the active energy output, and helps the thigh amputee to realize multiple actions such as load walking, ascending stairs, jumping and the like, so that more natural and steady walking experience is obtained. At present, in the field of research of dynamic thigh prostheses, basic actions including plantarflexion, dorsiflexion, pedaling and the like in the process of simply simulating human walking have been realized. Walking is a highly coupled system of a person, a thigh prosthesis and the environment, and the existing thigh prosthesis lacks the perception of the environment of the road condition ahead, so that the movement capability of amputees in the real environment is limited to a great extent. The accurate identification of the human body intention is an important bridge for human body-artificial limb-environment fusion, the complexity of a real walking environment aggravates the control difficulty of a power artificial limb, and the rapid and accurate identification of the motion intention is beneficial to the seamless switching of the motion mode of the artificial limb, so that an amputee can walk in the complex walking environment.

The existing human body intention recognition research is valuable, but most of the research focuses on a plurality of specific movement modes, only detects the current human body movement state, lacks an information interaction channel between the artificial limb and the environment, and is difficult to forecast the terrain and road condition information in advance. The daily complex and varied forms bring serious challenges to the movement intention recognition, so that a sensor with an environment sensing function is also needed to form a man-machine-ring fusion loop, and further the predictive adjustment control parameters of the power thigh artificial limb are assisted. Therefore, a child vision control system for fusing an environment and a prosthetic limb loop is innovatively provided by a adult team of the university of south science and technology, the environment type and the topographic parameters of the prosthetic limb are accurately identified, and the walking intention of a human body is robustly predicted. At present, a visual sensor has built a link between an artificial limb and an environment, can accurately acquire topographic parameters and categories, but still lacks self and topographic azimuth information which can be observed by human eyes, so that spatial references of human body-artificial limb-environment three are required to be uniformly represented, the position and the gesture of an amputee in the environment where the artificial limb is worn by the amputee are accurately perceived, and a foundation is laid for the motion planning of a dynamic thigh artificial limb under complex and changeable topography.

Disclosure of Invention

Aiming at the defects of the prior art, the application aims to provide a man-machine ring kinematics parameter self-calibration method and system of a power artificial limb, which aims to solve the problems that the power artificial limb lacks self and terrain azimuth information which can be observed by human eyes and cannot uniformly represent space references of human body, artificial limb and environment, so that the position and the gesture of the power artificial limb in a man-machine ring are difficult to accurately sense.

In order to achieve the above object, in a first aspect, the present application provides a method for self-calibrating a kinematic parameter of a man-machine ring of a powered artificial limb, the powered artificial limb is used to be worn under a hip joint of a user, an imaging device is fixed on the powered artificial limb, the imaging device is used to acquire topographic information of surrounding environment, the method includes the following steps:

Respectively establishing a user hip joint coordinate system, an imaging equipment coordinate system, a prosthetic knee joint coordinate system, a prosthetic ankle joint coordinate system, a prosthetic foot end coordinate system and a Cartesian coordinate system; the Cartesian coordinate system is established on the calibration plate;

When the hip joint, the artificial limb knee joint and the artificial limb ankle joint of the user rotate respectively, a first homogeneous transformation matrix between a hip joint fixing coordinate system and a coordinate system of the imaging equipment, a second homogeneous transformation matrix between a knee joint fixing coordinate system and an artificial limb foot end coordinate system and a third homogeneous transformation matrix between an ankle joint fixing coordinate system and an artificial limb foot end coordinate system are obtained based on the position and posture information of the calibration plate acquired by the imaging equipment; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates;

When the power artificial limb is matched with a user to walk, the pose information of the hip joint and the foot end of the artificial limb under the Cartesian coordinate system is determined based on the first, second and third homogeneous transformation matrixes after compensation, the real-time rotation angle of the hip joint, the pose information of the environment shot by the camera equipment, the real-time rotation angle of the knee joint, the real-time rotation angle of the ankle joint and the fourth and fifth homogeneous transformation matrixes, so that the self-calibration of kinematic parameters is realized, and the space references of the user, the environment and the power artificial limb are unified.

In one possible implementation, the first level transformation matrix is established by:

connecting a camera equipment coordinate system, a hip joint coordinate system and a Cartesian coordinate system to form a self-sampling coordinate tether;

When the hip joint of the user rotates, controlling the imaging device to record the pose of the calibration plate in the imaging device coordinate system, and then establishing a first relation among the hip joint coordinate system, the imaging device coordinate system and the Cartesian coordinate system:

Wherein, Representing a homogeneous transformation matrix between the hip coordinate system and the Cartesian coordinate system; a rotation matrix representing the hip joint; Representing a first level-shift matrix; The homogeneous transformation matrix between the coordinate system of the image pickup device and the Cartesian coordinate system is represented, and the homogeneous transformation matrix is obtained according to the pose of the calibration plate in the coordinate system of the image pickup device.

In one possible implementation, the second homogeneous transformation matrix is established by:

controlling the rotation of the artificial limb knee joint, controlling the imaging equipment to record the pose information of the calibration plate in the coordinate system of the imaging equipment, and then establishing a second relation among the coordinate system of the knee joint, the coordinate system of the imaging equipment and the coordinate system of the foot end:

when the knee joint rotates, the hip joint and the ankle joint are both in a fixed state, and the calibration plate is arranged at the foot end of the artificial limb, so that the Cartesian coordinate system is overlapped with the coordinate system of the foot end; represents a homogeneous transformation matrix between the knee joint coordinate system and the imaging device coordinate system, Representing a rotation matrix of the knee joint,Representing a second homogeneous transformation matrix,The homogeneous transformation matrix between the foot end coordinate system and the camera equipment coordinate system is represented and obtained through the pose of the calibration plate in the camera equipment coordinate system.

In one possible implementation, the third homogeneous transformation matrix is established by:

Controlling the rotation of the artificial limb ankle joint, controlling the imaging equipment to record the pose information of the calibration plate in the coordinate system of the imaging equipment, and then establishing a third relation among the ankle joint coordinate system, the coordinate system of the imaging equipment and the foot end coordinate system:

when the ankle joint rotates, the hip joint and the knee joint are both in a fixed state, and the calibration plate is arranged at the foot end of the artificial limb, so that the Cartesian coordinate system is overlapped with the coordinate system of the foot end; Represents a homogeneous transformation matrix between the ankle joint coordinate system and the imaging apparatus coordinate system, Representing a rotation matrix of the ankle joint,Representing a third homogeneous transformation matrix.

The rotation angle of the hip joint can be obtained by a sensor. The rotation angles of the knee joint and the ankle joint of the artificial limb can be obtained by directly reading the internal data by the power artificial limb.

In one possible implementation, the first, second or third homogeneous transformation matrix is determined by:

Combining first relation formulas corresponding to different joint rotation angles of the hip joint to obtain a kinematic parameter identification equation of a first unified transformation matrix;

Or the second relation formulas corresponding to the rotation angles of different knee joints are combined to obtain a kinematic parameter identification equation of a second homogeneous transformation matrix;

or combining third relation formulas corresponding to different joint rotation angles of the ankle joint to obtain a kinematic parameter identification equation of a third homogeneous transformation matrix;

and (3) in the equation, disassembling the first homogeneous transformation matrix, the second homogeneous transformation matrix or the third homogeneous transformation matrix into a matrix comprising a rotation matrix and a position matrix, and then solving a closed-loop solution of the rotation matrix and the position matrix by combining the rotation matrix corresponding to the joint rotation angle and the calibration plate pose acquired by the imaging equipment to obtain the closed-loop solution of the first homogeneous transformation matrix, the second homogeneous transformation matrix or the third homogeneous transformation matrix.

In one possible implementation manner, the compensated first, second and third homogeneous transformation matrices are obtained by the following steps:

Unitizing a rotation matrix corresponding to the closed-loop solution of the alignment transformation matrix, and performing angle inverse solution on the unitized rotation matrix to obtain RPY angle parameters of the rotation matrix;

Using the mean value of the square sum of the compensation residual errors as an objective function, adopting a particle swarm optimization algorithm to iteratively search for the angle error of the RPY angle parameter of the matched original rotation matrix, and compensating for the closed-loop solution of the alignment transformation matrix to make the rotation matrix orthogonal;

and determining a position matrix and a rotation matrix with orthogonality under the optimal angle parameters according to the angle errors obtained by searching, and updating the corresponding homogeneous transformation matrix to obtain a first homogeneous transformation matrix, a second homogeneous transformation matrix and a third homogeneous transformation matrix after compensation.

In one possible implementation, the fourth and fifth homogeneous transformation matrices are obtained by:

Multiplying the homogeneous transformation matrix between the foot end coordinate system and the camera equipment coordinate system during knee joint rotation by the compensated second homogeneous transformation matrix to obtain a fourth homogeneous transformation matrix;

And multiplying the inverse matrix of the compensated second homogeneous transformation matrix by the compensated third homogeneous transformation matrix, and then multiplying the inverse matrix by a rotation matrix corresponding to the ankle angle when the knee joint rotates to obtain a fifth homogeneous transformation matrix.

In one possible implementation manner, the pose information of the hip joint and the prosthetic foot end in the cartesian coordinate system is specifically:

Wherein, A pose matrix of the hip joint of the user in a Cartesian coordinate system; a pose matrix of an environment shot by the camera equipment; the method comprises the steps that a homogeneous transformation matrix between a hip joint coordinate system and an imaging equipment coordinate system is obtained by multiplying a compensated first homogeneous transformation matrix by a rotation matrix corresponding to a real-time rotation angle of a hip joint; The pose matrix of the prosthetic foot end in a Cartesian coordinate system; the homogeneous transformation matrix between the ankle joint coordinate system and the foot end coordinate system is obtained by multiplying the compensated third homogeneous transformation matrix by a rotation matrix corresponding to the real-time rotation angle of the ankle joint; is a homogeneous transformation matrix between the knee joint coordinate system and the ankle joint coordinate system, Is a homogeneous transformation matrix between the coordinate system of the image pickup device and the coordinate system of the knee joint,Is thatAn inverse matrix of (a);

Wherein, For the fourth homogeneous transformation matrix,A fifth homogeneous transformation matrix; is a rotation matrix corresponding to the real-time rotation angle of the knee joint, Is the real-time rotation angle of the knee joint in the walking process.

In a second aspect, the present application provides a system for self-calibrating a human-computer loop kinematic parameter of a powered prosthesis, the powered prosthesis being configured to be worn under a hip joint of a user, an imaging device being fixed on the powered prosthesis, the imaging device being configured to obtain topographic information of a surrounding environment, the system comprising:

The coordinate system establishing module is used for respectively establishing a user hip joint coordinate system, a camera equipment coordinate system, a prosthetic knee joint coordinate system, a prosthetic ankle joint coordinate system, a prosthetic foot end coordinate system and a Cartesian coordinate system; the Cartesian coordinate system is established on the calibration plate;

The transformation matrix solving module is used for obtaining a first homogeneous transformation matrix between a hip joint fixing coordinate system and a coordinate system of the imaging equipment, a second homogeneous transformation matrix between a knee joint fixing coordinate system and a coordinate system of the prosthetic foot end and a third homogeneous transformation matrix between an ankle joint fixing coordinate system and the coordinate system of the prosthetic foot end based on the calibration plate pose information acquired by the imaging equipment when the hip joint, the prosthetic knee joint and the prosthetic ankle joint of a user rotate respectively; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates;

The parameter self-calibration module is used for determining pose information of the hip joint and the foot end of the artificial limb under the Cartesian coordinate system based on the Cartesian coordinate system when the power artificial limb is matched with a user to walk, based on the compensated first, second and third homogeneous transformation matrixes, the real-time rotation angle of the hip joint, pose information of the environment shot by the camera equipment, the real-time rotation angle of the knee joint, the real-time rotation angle of the ankle joint and the fourth and fifth homogeneous transformation matrixes, so that the kinematic parameter self-calibration is realized, and the spatial references of the user, the environment and the power artificial limb are unified.

In a third aspect, the present application provides an electronic device comprising: at least one memory for storing a program; at least one processor for executing a memory-stored program, which when executed is adapted to carry out the method described in the first aspect or any one of the possible implementations of the first aspect.

In a fourth aspect, the present application provides a computer readable storage medium storing a computer program which, when run on a processor, causes the processor to perform the method described in the first aspect or any one of the possible implementations of the first aspect.

In a fifth aspect, the application provides a computer program product which, when run on a processor, causes the processor to perform the method described in the first aspect or any one of the possible implementations of the first aspect.

In general, the above technical solutions conceived by the present application have the following compared with the prior art

The beneficial effects are that:

The application provides a man-machine loop kinematics parameter self-calibration method and system of a power artificial limb, which constructs a geometric constraint measurement space of man-machine rigid-flexible coupling by constructing a space dimension chain with a sensor self-sampling characteristic of camera equipment, thereby avoiding pose errors caused by arrangement of sampling points of an external instrument, obtaining kinematics parameters by designing a closed loop-searching step-by-step identification algorithm, and realizing accurate unification of human body-artificial limb-environment space references. In addition, the method accurately transmits motion information of the artificial limb, the amputee and the environment, conforms to the trend of human-machine-environment fusion, and lays a good foundation for planning motion control of the power thigh artificial limb under complex terrain.

Drawings

FIG. 1 is a flow chart of a method for self-calibrating kinematic parameters of a powered prosthesis according to an embodiment of the present application;

FIG. 2 is a relative transformation matrix and coordinate tether for a human body, prosthesis and environment provided by an embodiment of the present application; (a) Is a relative coordinate system between man-machine rings of the power thigh artificial limb; (b) The system is a coordinate system of each connecting rod of the man-machine ring established based on DH rule;

FIG. 3 is a schematic diagram of a motion chain and sampling procedure of a human hip joint self-sampling coordinate system according to an embodiment of the present application;

FIG. 4 is a schematic diagram of a motion chain and a sampling procedure of a self-sampling coordinate system of a prosthetic ankle joint according to an embodiment of the present application;

FIG. 5 is a schematic diagram of a motion chain and sampling procedure of a self-sampling coordinate system of a prosthetic knee joint according to an embodiment of the present application;

FIG. 6 is a schematic illustration of an experimental setup provided by an embodiment of the present application;

FIG. 7 is a schematic diagram of an estimated foot end position error according to an embodiment of the present application;

FIG. 8 is a diagram of a dynamic artificial limb kinematic parameter self-calibration system according to an embodiment of the application.

Detailed Description

The present application will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present application more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.

The terms "first" and "second" and the like in the description and in the claims are used for distinguishing between different objects and not for describing a particular sequential order of objects. For example, the first and second homogeneous matrices, etc. are used to distinguish between different homogeneous matrices, and are not used to describe a particular order of homogeneous matrices.

In embodiments of the application, words such as "exemplary" or "such as" are used to mean serving as an example, instance, or illustration. Any embodiment or design described herein as "exemplary" or "e.g." in an embodiment should not be taken as preferred or advantageous over other embodiments or designs. Rather, the use of words such as "exemplary" or "such as" is intended to present related concepts in a concrete fashion.

The application provides a human-machine-loop kinematics parameter self-calibration method of a power thigh artificial limb, which constructs a geometric constraint measurement space of human-machine rigid-flexible coupling by constructing a space dimension chain with a self-sampling characteristic of a camera sensor, thereby avoiding pose errors caused by arranging sampling points of an external instrument, obtaining kinematics parameters by designing a step-by-step identification algorithm of closed loop-search, and realizing accurate unification of human body-artificial limb-environment space references. In addition, the scheme accurately transmits motion information of the artificial limb, the amputee and the environment, conforms to the trend of human-machine-environment fusion, and lays a good foundation for planning motion control of the power thigh artificial limb under complex terrain.

FIG. 1 is a flow chart of a method for self-calibrating human-machine loop kinematics parameters of a powered artificial limb according to an embodiment of the application; as shown in fig. 1, the power artificial limb is used for being worn below the hip joint of a user, and an imaging device is fixed on the power artificial limb and used for acquiring the topographic information of the surrounding environment, and the method comprises the following steps:

S101, respectively establishing a user hip joint coordinate system, a camera equipment coordinate system, a prosthetic knee joint coordinate system, a prosthetic ankle joint coordinate system, a prosthetic foot end coordinate system and a Cartesian coordinate system; the Cartesian coordinate system is established on the calibration plate;

S102, when a hip joint, an artificial limb knee joint and an artificial limb ankle joint of a user rotate respectively, a first homogeneous transformation matrix between a hip joint fixing coordinate system and a coordinate system of the imaging equipment, a second homogeneous transformation matrix between a knee joint fixing coordinate system and an artificial limb foot end coordinate system and a third homogeneous transformation matrix between an ankle joint fixing coordinate system and an artificial limb foot end coordinate system are obtained based on the position and posture information of a calibration plate acquired by the imaging equipment; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates;

And S103, when the power artificial limb is matched with a user to walk, the pose information of the hip joint and the foot end of the artificial limb under the Cartesian coordinate system is determined based on the first, second and third homogeneous transformation matrixes, the real-time rotation angle of the hip joint, the pose information of the environment shot by the camera equipment, the real-time rotation angle of the knee joint, the real-time rotation angle of the ankle joint and the fourth and fifth homogeneous transformation matrixes after compensation, so that the self calibration of kinematic parameters is realized, and the spatial references of the user, the environment and the power artificial limb are unified.

The following examples develop an introduction of how to build a self-sampling kinematic model in particular:

An information bridge between the environment and the powered knee joint prosthesis is established by means of a depth camera mounted on the prosthetic socket, wherein the pose of the camera can be acquired by means of a calibration plate. The relative transformation matrix and coordinate tethers for the human body, prosthesis and environment are shown in fig. 2. The intermediate coordinate system is the mounting coordinate system of the depth camera on the prosthesis receiving cavity. The camera sensors measure position data in the surrounding terrain and can be used to construct a kinematic self-sampling size chain. The DH law uses four parameters a _m-1,α_m-1,d_m,θ_m to describe the geometric relationship between adjacent link coordinate systems { m } and { m-1}, and defines the homogeneous transformation matrix of the link coordinate system { m } relative to { m-1}, as follows:

Meanwhile, the homogeneous transformation matrix between the link coordinate systems { m } and { m-1} satisfies the following inverse transformation:

Where a _m-1 = distance measured along x _m-1 from z _m-1 to z _m; α _m-1 = angle of rotation about x _m-1 from z _m-1 to z _m; d _m = distance measured along z _m from x _m-1 to x _m; θ _m =angle of rotation around z _m from x _m-1 to x _m.

Specifically, the DH rule proposed by Danevit Harttnberg is to build 4*4 homogeneous transformation matrix for the link coordinate system at each joint to represent the relationship between the link coordinate system at each joint and the previous link coordinate system, and multiply the homogeneous transformation matrix at all joints successively, and finally calculate the change matrix of the terminal coordinate system represented by the base coordinate system.

The powered prosthesis is, for example, worn under the hip joint of the human body by means of a receiving cavity or an L-shaped bracket, the powered prosthesis comprising a knee joint and an ankle joint, the camera device being fixed to the receiving cavity or the L-shaped bracket of the powered prosthesis.

For example, the user may be an amputee, or other experimenter, or the like. The image capturing device may be a camera or other equipment for obtaining topographic information, and the scheme provided by the application will be described below by taking a user as a normal person and taking the image capturing device as a camera.

Referring to FIG. 2, the relative coordinate system between the powered thigh prosthetic human ring of FIG. 2 (a); (b) And a coordinate system of each connecting rod of the man-machine ring is established based on DH rule. The relative coordinate system between amputee, prosthesis, environment can be reduced to fig. 2 (a), where the coordinate systems { H }, { E }, { V }, { R } represent the human, environment, vision, and prosthesis coordinate systems, respectively; t _H-V、T_R-V、T_V-E and T _E-R represent homogeneous transformation matrices between different coordinate systems. The geometrical relationship between the links of the man-machine ring of the powered thigh prosthesis is modeled in detail by DH rule, as shown in (b) of FIG. 2, wherein the coordinate systems {1}, {2}, {3}, {5}, { C } respectively represent hip joint, knee joint, ankle joint, camera and Cartesian coordinate systems, and the coordinate system {4} is set to represent the intermediate coordinate system where the camera is fixedly connected with the receiving cavity. The Cartesian coordinate system is established on the calibration plate.

According to the characteristics of the rotary joint and the self-sampling characteristic of the camera sensor, a self-sampling kinematic model is established. T _i ^iu (i=1, 2, 3) represents a transformation matrix from a rotational coordinate system { i } (rotating with the joint axis) to a fixed rotational coordinate system { iu } (i.e., not rotating with the joint axis). According to the general formula of the homogeneous transformation matrix of the formula (1), the homogeneous transformation matrix of the hip joint, the knee joint and the ankle joint, which are self-sampled by a camera, can be obtained as follows:

Wherein,

Wherein the method comprises the steps ofThe DH parameter representing the coordinate system {5} relative to the coordinate system { iu } is a fixed value.

In order to obtain a homogeneous transformation matrix of the camera coordinate system and the human hip joint coordinate system, the camera coordinate system, the hip joint coordinate system and the Cartesian coordinate system are connected to form a self-sampling coordinate tether, a designed hip joint sampling program is shown in fig. 3, and the Cartesian coordinate system is established on a calibration plate. The rotation of the hip joint of the human body drives the camera to rotate and sample, the knee joint and the ankle joint of the artificial limb are kept not to rotate, the camera records the position of the calibration plate, and then the relation among the hip joint coordinate system, the camera coordinate system and the Cartesian coordinate system can be established as the following formula:

Wherein, Representing a homogeneous transformation matrix between the hip coordinate system and the cartesian coordinate system,Representing a homogeneous transformation matrix between the camera coordinate system and the cartesian coordinate system,Representing a homogeneous transformation matrix between the hip coordinate system and the camera coordinate system,Representing the homogeneous transformation matrix between the hip joint fixation coordinate system and the camera coordinate system, T ₁ ^1u representing the rotation matrix of the hip joint.

In order to calculate the kinematic parameters of the artificial limb ankle joint, a camera coordinate system, an artificial limb ankle joint coordinate system and a foot coordinate system are connected to form a self-sampling coordinate tether. The calibration plate is placed at the foot end, a Cartesian coordinate system where the calibration plate is located is overlapped with the coordinate system of the foot end, the position of the calibration plate in the coordinate system of the camera is measured by fixing the human hip joint, the knee joint of the artificial limb and the camera on the L-shaped bracket, rotating the ankle joint of the artificial limb, and the sampling process is shown in figure 4. Then, the relationship among the ankle coordinate system, the camera coordinate system, and the foot coordinate system may be established as the following formula:

Wherein, Represents a homogeneous transformation matrix between the ankle joint coordinate system and the camera coordinate system,Representing a homogeneous transformation matrix between the foot coordinate system and the camera coordinate system,Represents a homogeneous transformation matrix between the ankle coordinate system and the foot coordinate system,Represents a homogeneous transformation matrix between the ankle joint fixation coordinate system and the foot coordinate system,Representing a rotation matrix of the ankle joint.

In particular, the method comprises the steps of,The position and the posture of the calibration plate are obtained through the camera, and the technology is integrated in the camera and can be directly read.

Since the shoe body and the artificial foot have elasticity, the length from the ankle to the heel is compressed in the standing stage. The toe cap is not easy to deform, so that the toe is set as a foot coordinate system, and as shown in fig. 5, similar to ankle joint self-sampling, in order to obtain the kinematics parameters of the knee joint, a sampling program for fixing a human hip joint, a camera on an L-shaped bracket and a prosthetic ankle joint and rotating the knee joint is designed. The relationship between the knee coordinate system, the camera coordinate system, and the foot coordinate system may be established as the following formula:

Wherein, Representing a homogeneous transformation matrix between the knee coordinate system and the camera coordinate system,Representing a homogeneous transformation matrix between the knee coordinate system and the foot coordinate system,Representing a homogeneous transformation matrix between the knee joint fixation coordinate system and the foot coordinate system,Representing the rotation matrix of the knee joint.

The following example is developed to introduce the step-wise identification of kinematic parameters based on a closed-loop-search algorithm:

Combining the formula (5), the homogeneous transformation matrix of the j and j+1 joint rotation angles of the hip joint can be obtained:

Combining equation (8) and equation (9), a parameter identification equation can be obtained:

AX-XB＝0 (10)

Wherein,

AndCan be calculated by taking the hip angle into equation (4).AndThe position and the posture of the calibration plate can be obtained through the camera, and the technology is integrated in the camera and can be directly read.

For easy solution, let:

and combining the formula (10), the solving equation of the rotation matrix R _X can be obtained:

R_AR_X-R_XR_B＝0 (13)

By the kronecker product transform:

Evec(R_X)＝0 (14)

Wherein,

The method is characterized by comprising the following steps of:

And then carrying out singular value decomposition on V _K to obtain the following steps:

The closed loop solution of equation (13), i.e., the rotation matrix R _X, is:

substituting the rotation matrix R _X into formula (10) and combining formula (12) to obtain the solution equation of the position matrix P _X is:

(R_A-I)P_X＝R_XP_B-P_A (18)

the closed-loop solution for the position matrix P _X can be found as:

P_X＝((R_A-I)^T(R_A-I))^-1(R_A-I)^T(R_XP_B-P_A) (19)

The identification equations for the knee and ankle kinematics parameters can be expressed as equation (20) and equation (21), and a closed-loop solution for the knee ankle rotation matrix is obtained by a method similar to that for solving the hip kinematics parameters.

However, due to noise effects, the homogeneous transformation matrix is obtainedIs not perfectly orthogonal. The application provides a closed loop-search step-by-step identification method, which is used for reducing uncertainty caused by orthogonalization of a rotation matrix. The rotation matrix may be represented by three independent RPY angle parameters α, β, γ and used as initial values for the smart search algorithm. First, let R _iu uniformly represent homogeneous transformation matrixP _iu represents a position matrix, and the rotation matrix R _iu is first unitized:

performing angle inverse solution on the formula (22), three angle parameters of the rotation matrix can be obtained as follows:

Let δα, δβ, δγ denote the errors of α, β, γ, and [ δpx, δpy, δpz ] denote the errors of the position matrix P _iu. The compensated orthogonal rotation matrix and position matrix can be expressed as:

The particle swarm optimization algorithm is an intelligent algorithm with simple structure and high convergence speed, and is used for searching errors of alpha, beta and gamma matched with the original rotation matrix. The particle swarm optimization algorithm performs search iteration on delta alpha, delta beta, delta gamma and delta p _x、δp_y、δp_z, and takes the mean value of the square sum of the compensation residual errors as an optimization target. For the mth (m=1, …, m) iteration, the calculation formula for the optimization objective function is:

Updating delta alpha, delta beta, delta gamma, delta px, delta py, delta pz and repeating the searching process until the maximum iteration number is reached or the optimization object converges to a given value, then obtaining an orthogonal matrix and a position matrix of the rotation matrix by the optimal angle parameters, and forming a homogeneous transformation matrix of the following formula:

So far, the homogeneous transformation matrix is calculated by the above formula Is a matrix of orthogonality:

In the knee joint self-sampling procedure, the calibrated obtained is utilized The homogeneous transformation matrix between the camera coordinate system and the knee joint fixation coordinate system can be obtained:

meanwhile, a homogeneous transformation matrix between the knee joint fixing coordinate system and the ankle joint coordinate system can be obtained:

Wherein θ _{Ankle-shaped ankle} is the angle of the ankle joint fixed in the knee self-sampling procedure.

The following embodiments are developed to introduce real-time perception of motion pose of human body and power thigh artificial limb:

Will be identified to obtain Multiplying the rotation matrix of the corresponding joint rotation angle to obtain the human hip joint coordinate system and the camera coordinate system, the prosthetic knee joint coordinate system and the foot end coordinate system, and the homogeneous transformation matrix between the prosthetic ankle joint coordinate system and the foot end coordinate system

Wherein, T _i ^iu (i=1, 2, 3) can be calculated by bringing the real-time rotation angles of the hip joint, the knee joint and the ankle joint into the formula (4),The real-time hip joint, knee joint and ankle joint angles in the actual walking process.

When the artificial limb actually walks in an outdoor environment, the Cartesian coordinate line is used as a reference, the accurate man-machine-ring kinematics parameters are obtained through calibration by a designed method, and the pose matrix of the hip joint of the human body can be obtained through calculation:

Wherein, The pose matrix of the terrain is obtained for the camera identification, and the part is the prior art and is not repeated.

The pose of the prosthetic foot end under the Cartesian coordinate system can be calculated:

Wherein,

Because the camera coordinate system does not rotate during the motion of the artificial limbAnd the camera coordinate system does not rotate relative to the knee joint fixed coordinate system,Thus, the first and second substrates are bonded together,

After the man-machine ring standard is unified, the thigh artificial limb can sense the position of the thigh artificial limb relative to the human body and the environment.

Further, after the man-machine ring standard is unified, accurate motion planning of the power artificial limb can be achieved according to the pose matrix of the hip joint and the pose matrix of the foot end under the Cartesian coordinate system. The specific exercise planning method can be described in the prior art, for example, in patent document CN 116421372a.

In one embodiment, a healthy person wears a powered thigh prosthesis through an L-shaped bracket for an experiment, the experimental setup being shown in fig. 6. In particular, referring to fig. 6, the hip angle may be obtained by an inertial sensor (Inertial Measurement Unit, IMU) mounted on an L-shaped bracket. Other sensors or other means for obtaining hip joint angle may be selected by those skilled in the art depending on the actual situation.

The dynamic thigh artificial limb senses the position accuracy of the dynamic thigh artificial limb in the environment as an evaluation index, experimental verification is carried out, the foot end position measured by the dynamic capturing equipment is taken as a true value, the foot end position error in the estimated advancing direction is shown in figure 7, the average position error in the x direction is 3.07cm plus or minus 0.83cm, the average position error in the y direction is 2.29cm plus or minus 0.41cm, the estimation accuracy is high, and the effectiveness of the method provided by the application is proved.

FIG. 8 is a diagram of a kinematic parameter self-calibration system architecture for a powered prosthesis according to an embodiment of the present application; as shown in fig. 8, includes:

the coordinate system establishment module 810 is configured to establish a user hip joint coordinate system, an imaging device coordinate system, a prosthetic knee joint coordinate system, a prosthetic ankle joint coordinate system, a prosthetic foot end coordinate system, and a cartesian coordinate system, respectively; the Cartesian coordinate system is established on the calibration plate;

The transformation matrix solving module 820 is configured to obtain, when the hip joint, the prosthetic knee joint and the prosthetic ankle joint of the user rotate respectively, a first homogeneous transformation matrix between the hip joint fixing coordinate system and the coordinate system of the imaging device, a second homogeneous transformation matrix between the knee joint fixing coordinate system and the prosthetic foot end coordinate system, and a third homogeneous transformation matrix between the ankle joint fixing coordinate system and the prosthetic foot end coordinate system based on the calibration plate pose information acquired by the imaging device; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates;

The parameter self-calibration module 830 is configured to determine pose information of the hip joint and the foot end of the prosthesis in the cartesian coordinate system based on the compensated first, second and third homogeneous transformation matrices, the real-time rotation angle of the hip joint, pose information of the environment photographed by the photographing device, the real-time rotation angle of the knee joint, the real-time rotation angle of the ankle joint, and the fourth and fifth homogeneous transformation matrices when the power prosthesis is used for walking in cooperation with the user, thereby realizing the self-calibration of kinematic parameters and unifying spatial references of the user, the environment and the power prosthesis.

It can be understood that the detailed functional implementation of each module may be referred to the description in the foregoing method embodiment, and will not be repeated herein.

It should be understood that, the system is used to execute the method in the foregoing embodiment, and corresponding program modules in the system implement principles and technical effects similar to those described in the foregoing method, and the working process of the system may refer to the corresponding process in the foregoing method, which is not repeated herein.

Based on the method in the above embodiment, the embodiment of the application provides an electronic device. The apparatus may include: at least one memory for storing programs and at least one processor for executing the programs stored by the memory. Wherein the processor is adapted to perform the method described in the above embodiments when the program stored in the memory is executed.

Based on the method in the above embodiment, the embodiment of the present application provides a computer-readable storage medium storing a computer program, which when executed on a processor, causes the processor to perform the method in the above embodiment.

Based on the method in the above embodiments, an embodiment of the present application provides a computer program product, which when run on a processor causes the processor to perform the method in the above embodiments.

It is to be appreciated that the processor in embodiments of the present application may be a central processing unit (centralprocessing unit, CPU), other general purpose processor, digital signal processor (digital signalprocessor, DSP), application Specific Integrated Circuit (ASIC), field programmable gate array (field programmable GATE ARRAY, FPGA) or other programmable logic device, transistor logic device, hardware components, or any combination thereof. The general purpose processor may be a microprocessor, but in the alternative, it may be any conventional processor.

The steps of the method in the embodiment of the present application may be implemented by hardware, or may be implemented by executing software instructions by a processor. The software instructions may be comprised of corresponding software modules that may be stored in random access memory (random access memory, RAM), flash memory, read-only memory (ROM), programmable ROM (PROM), erasable programmable ROM (erasable PROM, EPROM), electrically Erasable Programmable ROM (EEPROM), registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor such the processor can read information from, and write information to, the storage medium. In the alternative, the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC.

In the above embodiments, it may be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When loaded and executed on a computer, produces a flow or function in accordance with embodiments of the present application, in whole or in part. The computer may be a general purpose computer, a special purpose computer, a computer network, or other programmable apparatus. The computer instructions may be stored in or transmitted across a computer-readable storage medium. The computer instructions may be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by a wired (e.g., coaxial cable, fiber optic, digital Subscriber Line (DSL)), or wireless (e.g., infrared, wireless, microwave, etc.). The computer readable storage medium may be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. that contains an integration of one or more available media. The usable medium may be a magnetic medium (e.g., floppy disk, hard disk, tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid State Drive (SSD)), etc.

It will be appreciated that the various numerical numbers referred to in the embodiments of the present application are merely for ease of description and are not intended to limit the scope of the embodiments of the present application.

It will be readily appreciated by those skilled in the art that the foregoing description is merely a preferred embodiment of the application and is not intended to limit the application, but any modifications, equivalents, improvements or alternatives falling within the spirit and principles of the application are intended to be included within the scope of the application.

Claims

1. The method for self-calibrating the man-machine loop kinematics parameters of the power artificial limb is used for being worn below the hip joint of a user, and the power artificial limb is fixedly provided with camera equipment which is used for acquiring the topographic information of the surrounding environment, and is characterized by comprising the following steps:

When the hip joint, the artificial limb knee joint and the artificial limb ankle joint of the user rotate respectively, a first homogeneous transformation matrix between a hip joint fixing coordinate system and a coordinate system of the imaging equipment, a second homogeneous transformation matrix between a knee joint fixing coordinate system and an artificial limb foot end coordinate system and a third homogeneous transformation matrix between an ankle joint fixing coordinate system and an artificial limb foot end coordinate system are obtained based on the position and posture information of the calibration plate acquired by the imaging equipment; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates; the first uniform transformation matrix is established by the following steps: connecting a camera equipment coordinate system, a hip joint coordinate system and a Cartesian coordinate system to form a self-sampling coordinate tether; when the hip joint of the user rotates, controlling the imaging device to record the pose of the calibration plate in the imaging device coordinate system, and then establishing a first relation among the hip joint coordinate system, the imaging device coordinate system and the Cartesian coordinate system: ; wherein, Representing a homogeneous transformation matrix between the hip coordinate system and the Cartesian coordinate system; a rotation matrix representing the hip joint; Representing a first level-shift matrix; the homogeneous transformation matrix between the coordinate system of the image pickup device and the Cartesian coordinate system is represented, and the homogeneous transformation matrix is obtained according to the pose of the calibration plate in the coordinate system of the image pickup device;

2. The method of claim 1, wherein the second homogeneous transformation matrix is established by:

3. The method of claim 1, wherein the third homogeneous transformation matrix is established by:

when the ankle joint rotates, the hip joint and the knee joint are both in a fixed state, and the calibration plate is arranged at the foot end of the artificial limb, so that the Cartesian coordinate system is overlapped with the coordinate system of the foot end; Represents a homogeneous transformation matrix between the ankle joint coordinate system and the imaging apparatus coordinate system, Representing homogeneous transformation matrix between foot end coordinate system and camera equipment coordinate system, obtaining by calibrating position of plate in camera equipment coordinate system,Representing a rotation matrix of the ankle joint,Representing a third homogeneous transformation matrix.

4. A method according to any one of claims 2 to 3, characterized in that the first, second or third homogeneous transformation matrix is determined by:

5. The method of claim 4, wherein the compensated first, second and third homogeneous transformation matrices are obtained by:

6. The method of claim 2, wherein the fourth and fifth homogeneous transformation matrices are obtained by:

7. The method according to claim 1 or 6, wherein the pose information of the hip joint and prosthetic foot end in the cartesian coordinate system is specifically:

8. The utility model provides a man-machine ring kinematics parameter self calibration system of power artificial limb, power artificial limb is used for wearing in user's hip joint below, is fixed with camera equipment on the power artificial limb, camera equipment is used for acquireing the topography information of surrounding environment, its characterized in that, the system includes:

The transformation matrix solving module is used for obtaining a first homogeneous transformation matrix between a hip joint fixing coordinate system and a coordinate system of the imaging equipment, a second homogeneous transformation matrix between a knee joint fixing coordinate system and a coordinate system of the prosthetic foot end and a third homogeneous transformation matrix between an ankle joint fixing coordinate system and the coordinate system of the prosthetic foot end based on the calibration plate pose information acquired by the imaging equipment when the hip joint, the prosthetic knee joint and the prosthetic ankle joint of a user rotate respectively; compensating the first, second and third homogeneous transformation matrixes to make the rotation matrixes orthogonal to each other, and obtaining compensated first, second and third homogeneous transformation matrixes; obtaining a fourth homogeneous transformation matrix between the knee joint fixed coordinate system and the imaging equipment coordinate system based on the compensated second homogeneous transformation matrix, and determining a fifth homogeneous transformation matrix between the ankle joint coordinate system and the knee joint fixed coordinate system based on the compensated second homogeneous transformation matrix, the compensated third homogeneous transformation matrix and the angle of the ankle joint when the knee joint rotates; the first uniform transformation matrix is established by the following steps: connecting a camera equipment coordinate system, a hip joint coordinate system and a Cartesian coordinate system to form a self-sampling coordinate tether; when the hip joint of the user rotates, controlling the imaging device to record the pose of the calibration plate in the imaging device coordinate system, and then establishing a first relation among the hip joint coordinate system, the imaging device coordinate system and the Cartesian coordinate system: ; wherein, Representing a homogeneous transformation matrix between the hip coordinate system and the Cartesian coordinate system; a rotation matrix representing the hip joint; Representing a first level-shift matrix; the homogeneous transformation matrix between the coordinate system of the image pickup device and the Cartesian coordinate system is represented, and the homogeneous transformation matrix is obtained according to the pose of the calibration plate in the coordinate system of the image pickup device;

9. An electronic device, comprising:

at least one memory for storing a program;

At least one processor for executing the memory-stored program, which processor is adapted to perform the method according to any one of claims 1 to 7 when the memory-stored program is executed.