WO2021128787A1 - Positioning method and apparatus - Google Patents

Abstract

A positioning method and apparatus, wherein same are used for improving the positioning precision. The method comprises: determining first posture information of a movable end by means of image information acquired by a photographic apparatus (100); determining second posture information of the movable end by means of motion information acquired by a collector (200); and determining posture information of the movable end according to the first posture information and the second posture information. Posture information of a movable end is determined by means of fusing first posture information and second posture information, such that visual sensing information is acquired, and accurate positioning of a posture of the movable end of a mechanical arm (300) is realized in combination with kinematics information of the mechanical arm (300). In this way, accurate positioning can be realized, even under different working environments, without designing an additional mechanical structure.

Description

Method and device for positioning

Cross references to related applications

This application claims the priority of a Chinese patent application filed with the Chinese Patent Office, the application number is 201911340837.X, and the application name is "a method and device for positioning" on December 23, 2019. The entire content of the Chinese patent application is incorporated herein by reference. Applying.

Technical field

The present invention relates to the field of computer technology, in particular to a positioning method and device.

Background technique

Traditional server room inspection, operation and maintenance uses manual inspection, which not only requires heavy tasks and consumes a lot of manpower and material resources, but also often fails to detect abnormalities in time due to human negligence and other reasons, which affects the timely repair of equipment in the computer room. The use of robots to perform operation and maintenance tasks can carry out 24-hour uninterrupted inspection and operation and maintenance in unattended or less-attended computer rooms, which is convenient for timely detection of abnormal conditions of the equipment in the computer room.

When the robot performs operation and maintenance tasks in the computer room, such as performing actions such as restarting the server, plugging and unplugging the server hard disk, etc., it is necessary to drive the robot arm to move to a fixed position. However, the size of the buttons on the server and the thickness of the hard disk are in the millimeter level. In this case, if you want to accurately touch the button or pull out the hard disk, you need to accurately position the movable end of the robotic arm in real time to ensure that it can move. To the target location. In order to achieve accurate positioning, the technical solutions currently used are as follows:

Positioning is achieved by designing a specific mechanical structure or installing a sensing device at a specific location. On the one hand, this solution requires the design of a specific device (such as a three-dimensional laser scanner installed on a robotic arm) for positioning, which leads to an increase in the cost of the product. On the other hand, although its positioning accuracy in a fixed position is high, it is positioned in other positions The accuracy will be reduced.

In other words, the above solution requires an additional design of a specific positioning structure, which not only reduces the range of motion of the robotic arm, lacks versatility, but also increases the hardware cost, which is not conducive to popularization.

Summary of the invention

The present invention provides a positioning method and device to solve the problem of how to accurately position the movable end of a mechanical arm.

In a first aspect, the present invention provides a positioning method, which is suitable for a robot provided with a robotic arm, the robotic arm being arranged on a base of the robot; including:

Determine the first pose information of the movable end of the robot arm through the image information acquired by the camera device of the robot;

Determine the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm;

Determine the pose information of the movable end according to the first pose information and the second pose information.

In the above solution, the first pose information is obtained by collecting image information, the second pose information is obtained by collecting motion information, and the first pose information and the second pose information are fused to determine the pose information of the movable end. The vision sensor information is combined with the kinematics information of the robotic arm to accurately locate the movable end pose of the robotic arm. This method does not require the design of additional mechanical structures, and can achieve precise positioning even in different working environments.

Optionally, the determining the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm includes:

Obtain the movement information of each joint of the mechanical arm through the first collector, and determine the position information of the movable end according to the movement information of the joints and the position information of the base;

Acquiring the posture information of the movable end through the second collector;

According to the position information of the movable end and the posture information of the movable end, the second pose information of the movable end is determined.

In the above scheme, the position information of the movable end is obtained through the first collector, the posture information of the movable end is obtained through the second collector, and then the two are combined to obtain the pose of the movable end. This method takes into account the movement of the robot arm while positioning. Learning information can reduce the impact of external noise on positioning and help improve the robustness and accuracy of measurement results.

Optionally, the determining the position information of the movable end according to the movement information of each joint and the position information of the base includes:

Determine the first transformation matrix between the joints from the base to the movable end according to the angle information of the joints;

Determine the second coordinate of the movable end in the world coordinate system according to the first transformation matrix of each joint and the first coordinate of the base in the world coordinate system;

According to the position information of the movable end and the posture information of the movable end, determining the second pose information of the movable end includes:

Determine the rotation matrix of the movable end according to the posture information of the movable end;

Determining a second transformation matrix to transform from the base to the movable end according to the rotation matrix and the second coordinates;

Determine the second pose information of the movable end according to the second transformation matrix and the second coordinates.

In the above solution, the position information of the movable end is obtained by using the position information of the base and the transformation matrix between the joints, which can quickly and accurately obtain the position information of the movable end of the manipulator without adding additional mechanical structure.

Optionally, the second coordinate of the movable end in the world coordinate system is determined by formula (1):

Among them, [x _a , y _a , z _a , 1] are the coordinates of the base in the world coordinate system, and [x _h ′, y _h ′, z _h ′, 1] are the coordinates of the movable end of the robotic arm For the coordinates in the world coordinate system, ^n-1 T _n is the first transformation matrix between the joints.

Optionally, the second transformation matrix of the active end is determined by formula (2):

Among them, R _h =R(θ _roll )R(θ _pitch )R(θ _yaw ) is the rotation matrix, θ _roll is the roll angle _{of the movable end of the robot arm, θ pitch} is the pitch angle of the movable end of the robot arm, θ _yaw is the yaw angle of the movable end of the robotic arm.

Optionally, the second pose information of the movable end is determined by formula (3):

Formula (3) P _w ′ = ^w T _r ·P _r

Wherein, P _'W pose information of the second movable end, ^W T _r is a second movable end of the transformation matrix, P _r is the position of the base in the world coordinate system.

Optionally, the determining the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm includes:

Determine the state transition matrix of the robotic arm through the motion information acquired by the first collector;

Determine the input transfer matrix and the joint input matrix of the robot arm according to the control information corresponding to the motion information;

Determine the current second pose information of the movable end according to the second pose information, the state transition matrix, the input transition matrix, and the joint input matrix of the movable end at the previous moment.

The above solution predicts the current pose by using the second pose information of the active end at the last moment and the internal inertia of the robot arm at the last moment and the control information at the previous moment (such as the influence of the torque input by the motor on the current motion). Can improve the accuracy of positioning.

Optionally, the determining the pose information of the movable end according to the first pose information and the second pose information includes:

Determining the first error of the first pose information and the second error of the second pose information;

Determine the pose information of the movable end according to the first error, the second error, the first pose information, and the second pose information.

The above scheme, by using the first error, the second error, the first pose information, and the second pose information to determine the pose information of the active end, not only can predict the current pose of the active end, but also calculate the credibility of the predicted value , To further improve the accuracy of positioning.

Optionally, the pose information of the movable end is determined by formula (5):

Formula (5) P _w(i) = P′ _w(i) + K _i ·(P″ _w(i) -H·P′ _w(i) )

P′ _w(i)为第二姿态信息，P″ _w(i)为第一姿态信息，K _i为增益系数，H为摄像装置的观测矩阵，

为第二位姿信息的第二误差，R为相机测量的不确定度。 among them,

P′ _w(i) is the second posture information, P″ _w(i) is the first posture information, K _i is the gain coefficient, H is the observation matrix of the camera device,

Is the second error of the second pose information, and R is the uncertainty of the camera measurement.

Optionally, the second error of the second pose information is determined by formula (6):

Among them, Pi _-1 is the pose of the movable end at the previous moment, and V is the environmental noise.

In the second aspect, the present invention provides a positioning device, including:

An acquiring unit, configured to acquire image information through the camera device of the robot, and acquire movement information through a collector provided on the robotic arm;

The processing unit is used to determine the first posture information of the movable end of the robot arm through the image information acquired by the camera device of the robot, and determine the activity through the motion information acquired by the collector provided on the robot arm The second pose information of the mobile terminal determines the pose information of the movable terminal according to the first pose information and the second pose information.

Optionally, the processing unit is specifically configured to:

Obtain the movement information of each joint of the mechanical arm through the first collector, and determine the position information of the movable end according to the movement information of the joints and the position information of the base;

Acquiring the posture information of the movable end through the second collector;

According to the position information of the movable end and the posture information of the movable end, the second pose information of the movable end is determined.

Optionally, the processing unit is specifically configured to: determine a first transformation matrix between the joints from the base to the movable end according to the angle information of the joints;

Determine the second coordinate of the movable end in the world coordinate system according to the first transformation matrix of each joint and the first coordinate of the base in the world coordinate system;

According to the position information of the movable end and the posture information of the movable end, determining the second pose information of the movable end includes:

Determine the rotation matrix of the movable end according to the posture information of the movable end;

Determining a second transformation matrix to transform from the base to the movable end according to the rotation matrix and the second coordinates;

Determine the second pose information of the movable end according to the second transformation matrix and the second coordinates.

Optionally, the processing unit is specifically configured to determine the second coordinate of the movable end in the world coordinate system through formula (1):

Among them, [x _a , y _a , z _a , 1] are the coordinates of the base in the world coordinate system, and [x _h ′, y _h ′, z _h ′, 1] are the coordinates of the movable end of the robotic arm Coordinates in the world coordinate system, ^n-1 T _n is the first transformation matrix between the joints;

The second transformation matrix of the movable end is determined by formula (2):

Among them, R _h =R(θ _roll )R(θ _pitch )R(θ _yaw ) is the rotation matrix, θ _roll is the roll angle _{of the movable end of the robot arm, θ pitch} is the pitch angle of the movable end of the robot arm, θ _yaw is the yaw angle of the movable end of the robotic arm;

The second pose information of the movable end is determined by formula (3):

Formula (3) P _w ′ = ^w T _r ·P _r

Wherein, P _'W pose information of the second movable end, ^W T _r is a second movable end of the transformation matrix, P _r is the position of the base in the world coordinate system.

Optionally, the processing unit is specifically configured to: determine the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm, including:

Determine the state transition matrix of the robotic arm through the motion information acquired by the first collector;

Determine the input transfer matrix and the joint input matrix of the robot arm according to the control information corresponding to the motion information;

Determine the current second pose information of the movable end according to the second pose information, the state transition matrix, the input transition matrix, and the joint input matrix of the movable end at the previous moment.

Optionally, the processing unit is specifically configured to determine the current second pose information of the active end through formula (4):

Formula (4) P′ _w(i) =A·P _w(i-1) +B·u _i-1

Wherein, A is the state transition matrix of the robotic arm, B is the input transition matrix of the robotic arm, and u _i-1 is the joint input matrix of the i-1sth robotic arm.

Optionally, the processing unit is specifically configured to: the determining the pose information of the movable end according to the first pose information and the second pose information includes:

Determining the first error of the first pose information and the second error of the second pose information;

Determine the pose information of the movable end according to the first error, the second error, the first pose information, and the second pose information.

Optionally, the processing unit is specifically configured to determine the pose information of the movable end through formula (5):

Formula (5) P _w(i) = P′ _w(i) + K _i ·(P″ _w(i) -H·P′ _w(i) )

P′ _w(i)为第二姿态信息，P″ _w(i)为第一姿态 信息，K _i为增益系数，H为摄像装置的观测矩阵，

为第二位姿信息的第二误差，R为相机测量的不确定度。 among them,

P′ _w(i) is the second posture information, P″ _w(i) is the first posture information, K _i is the gain coefficient, H is the observation matrix of the camera device,

Is the second error of the second pose information, and R is the uncertainty of the camera measurement.

Optionally, the processing unit is specifically configured to determine the second error of the second pose information through formula (6):

Among them, Pi _-1 is the pose of the movable end at the previous moment, and V is the environmental noise.

In a third aspect, the present invention provides a computer controlled device, including:

Memory, used to store program instructions;

The processor is configured to call the program instructions stored in the memory, and execute the method described in the first aspect above according to the obtained program.

In a fourth aspect, the present invention provides a computer-readable non-volatile storage medium, including computer-readable instructions. When the computer reads and executes the computer-readable instructions, the computer is caused to execute the method described in the first aspect. .

Description of the drawings

In order to explain the technical solutions in the embodiments of the present invention more clearly, the following will briefly introduce the drawings needed in the description of the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained from these drawings without creative labor.

FIG. 1 is a schematic diagram of a system architecture provided by an embodiment of the present invention;

2 is a schematic flowchart of a positioning method provided by an embodiment of the present invention;

FIG. 3 is a schematic flowchart of a positioning method provided by an embodiment of the present invention;

4 is a schematic diagram of a positioning method provided by an embodiment of the present invention;

FIG. 5 is a schematic flowchart of a positioning method provided by an embodiment of the present invention;

FIG. 6 is a schematic flowchart of a positioning method provided by an embodiment of the present invention;

FIG. 7 is a schematic structural diagram of a positioning device provided by an embodiment of the present invention.

Detailed ways

In order to better understand the above technical solutions, the above technical solutions will be described in detail below in conjunction with the drawings and specific implementations of the specification. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed to the technical solutions of the present invention. Note, rather than limiting the technical solution of the present invention, the embodiments of the present invention and the technical features in the embodiments can be combined with each other under the condition of no conflict.

The positioning method provided by the embodiment of the present application may be applicable to a robot provided with a mechanical arm, wherein the mechanical arm may be set on the base of the robot. FIG. 1 exemplarily shows a system architecture to which an embodiment of the present invention is applicable. The system architecture may include a camera device 100, a collector 200, and a robotic arm 300.

Among them, the camera device 100 is used to obtain image information of the movable end of the robotic arm 300.

The collector 200 is arranged on the robotic arm 300 and is used to obtain movement information of the movable end of the robotic arm 300.

It should be noted that the imaging device 100 may be a vision sensor such as a monocular camera, a depth camera, etc. The structure shown in FIG. 1 is only an example, and the embodiment of the present invention does not limit this.

In order to better explain the foregoing embodiment, FIG. 2 exemplarily shows a flow of a positioning method, which may be executed by a positioning device.

As shown in Figure 2, the process specifically includes:

Step 201: Determine the first pose information of the movable end of the robot arm through the image information obtained by the camera of the robot.

It should be noted that the camera device may be a vision sensor such as a monocular camera and a depth camera.

Step 202: Determine the second pose information of the movable end through the movement information acquired by the collector set on the robotic arm.

Step 203: Determine the pose information of the movable end according to the first pose information and the second pose information.

In the above solution, the first pose information is obtained by collecting image information, the second pose information is obtained by collecting motion information, and the first pose information and the second pose information are merged to determine the pose information of the movable end. Acquire the visual sensor information and combine the kinematics information of the robotic arm to realize the precise positioning of the active end pose of the robotic arm. In this way, no additional mechanical structure is required, and precise positioning can be achieved in different working environments.

A specific implementation process of the above step 201 may be as shown in Fig. 3:

Step 301: Obtain the positions of multiple points at the movable end of the robotic arm.

Step 302: Determine the first pose information of the movable end of the robotic arm according to the positions of the multiple points.

In a possible implementation, the installation of the depth camera can be completed first, and the depth camera can be installed above the robotic arm. After the depth camera is installed, the positions of multiple points on the movable end of the robotic arm can be obtained later to generate a position matrix .

_{For example, obtain the position matrix M i} (i=1...4) of the four points at the movable end of the robotic arm in the camera coordinate system, and obtain a fixed point P on the movable end of the robotic arm from these four coordinates (here, it is preferred to take The center of the four points) is the coordinate P0 in the camera coordinate system.

Specifically, first determine whether the four points are occluded, and if occlusion occurs, use the remaining points to complete the measurement. When there is no occlusion in the four points, the position coordinates of P0 can be obtained by the following formula:

It should be noted that since P is located at the center of the four points, the coordinates P0 of the fixed point P can be obtained by averaging here. When the four points are occluded, the coordinates of the remaining points cannot be simply averaged. Instead, the coordinates of each point are first weighted and then averaged according to the positional relationship between the remaining points and P. For a simple example, for example, one of the four points is occluded, as shown in Figure 4, first take the average of the coordinates of the two points a and b, that is, the coordinates of e, and then take the average of the coordinates of the two points b and c That is, the coordinate of f, and then the position coordinate of P is obtained according to the ordinate of e and the abscissa of f. Of course, the simple example here takes two-dimensional coordinates as an example. In the actual process, each point takes the coordinates in space, that is, three-dimensional coordinates.

Further, the posture of the movable end of the robotic arm is completed by the following formula:

R _O ＝f(M _i )

Among them, f is the posture conversion function.

Based on the foregoing, the position and orientation coordinates of the free end of the robot arm in the camera coordinate system P _o converted to world coordinates, denoted by ".P _w" can be obtained by the following formula P _w:

P _w "= ^w T _c ·P _o

Among them, ^w T _c is the homogeneous transformation matrix that changes from the camera coordinate system to the world coordinate system.

It should be noted that the above content uses 4 points as an example. The embodiments of this application can take any number of points, such as 5 points, 6 points, etc. This application does not specifically limit this. Of course, only one point is selected. The more the number, the more accurate the position of the point P is obtained.

A specific implementation process of step 202 may be as shown in FIG. 5:

Step 501: Obtain motion information of each joint of the robotic arm through the first collector.

Step 502: Determine the position information of the movable end according to the movement information of each joint and the position information of the base.

Step 503: Obtain posture information of the mobile terminal through the second collector.

Step 504: Determine the second pose information of the mobile terminal according to the position information of the mobile terminal and the posture information of the mobile terminal.

In the above step 501, the first collector may be a code disc installed in each movement joint, or other sensors that can obtain movement information, which is not specifically limited in this application. The acquired motion information may be information that can indicate the motion parameters of each joint, such as joint angles and joint angular speeds, which are not specifically limited in this application.

Specifically, in step 502, the first transformation matrix between the joints from the base to the movable end may be determined first according to the angle information of each joint, and then the first transformation matrix between the base and the base is determined according to the first transformation matrix of each joint. The first coordinate in the world coordinate system determines the second coordinate of the movable end in the world coordinate system.

Further, the second coordinate of the movable end in the world coordinate system can be determined by formula (1):

Among them, [x _a ,y _a ,z _a ,1] are the coordinates of the base in the world coordinate system, [x _h ′,y _h ′,z _h ′,1] are the movable end of the robot arm in the world coordinate system coordinate of.

Based on the above content, to give a specific example, for example, the robot has 6 joints from its base to the movable end of the manipulator. The motion information of each joint is collected through the code disk of each motion joint, which mainly includes joint angle, joint angular velocity, etc. ^{The homogeneous transformation matrix 0} T ₁ , ¹ T ₂ , ² T ₃ , ³ T ₄ , ⁴ T ₅ , ⁵ T _{6 of} joint 1 to joint 6 can be obtained. Suppose the coordinates of the robot base in the world coordinate system are [x _a ,y _a ,z _a ,1], and the coordinates of the fixed point P of the movable end of the robot arm in the world coordinate system are [x _h ′,y _h ′, z _h ′,1], according to the principle of kinematics, we can get:

From this, the position information of the movable end of the robotic arm in the world coordinate system is obtained.

Further, in step 503, the second collector may be a sensor device such as a gyroscope that can collect attitude information of the movable end. The attitude information includes, but is not limited to, the pitch angle θ _pitch , the yaw angle θ _yaw , and the roll of the movable end of the robotic arm. Angle θ _roll, etc., this application does not specifically limit the second collector and the posture information collected by the second collector.

In the embodiment of the present application, a specific implementation process of step 504 is as follows:

First, determine the rotation matrix of the movable end according to the posture information of the movable end.

Then, according to the rotation matrix and the second coordinates, a second transformation matrix for transforming from the base to the movable end is determined.

Finally, according to the second transformation matrix and the second coordinates, the second pose information of the movable end is determined.

Specifically, for example, first collect the attitude information of the movable end of the manipulator measured by the inertial measuring instrument on the movable end of the manipulator to obtain the pitch angle θ _pitch , yaw angle θ _yaw and roll angle θ _{roll of the} movable end of the manipulator, thereby obtaining the mechanical _{The rotation matrix R h} of the movable end of the arm is:

R _h ＝R(θ _roll )R(θ _pitch )R(θ _yaw )

Further, the homogeneous transformation matrix from the base of the robot arm to the movable end of the robot arm can be obtained as:

Finally, the pose information of the movable end of the robotic arm in the world coordinate system can be obtained by the following formula:

P _w ′= ^w T _r ·P _r

Among them, P _r is the position of the robot arm base in the world coordinate system.

Based on the above content, the following briefly introduces the principle of the homogeneous transformation matrix wTr.

First of all, adding the coordinates of the movable end of the robotic arm in the world coordinate system to the transformation matrix is based on the principle of pure translation transformation, that is, moving in a constant posture in space. In this case, its direction unit vector remains the same. The direction remains unchanged, all changes are just the transformation of the origin of the coordinate system relative to the reference coordinate system.

Relative to the fixed reference coordinate system, the position of the new coordinate system can be represented by the original position vector of the original coordinate system plus the vector representing the displacement. If it is in matrix form, the representation of the new coordinate system can be obtained by multiplying the coordinate system by the transformation matrix, that is, the transformation matrix is as follows:

_{Wherein, x 'h, y' h} , z 'h flat is moved with respect to the amount of the reference coordinate axes of the three components. The first 3 columns of the matrix indicate no rotational movement, and the last column indicates translational movement.

Based on the above content, the rotation matrix R _h is added to the first 3 columns of the matrix to obtain a homogeneous transformation matrix.

In the above scheme, the position information of the movable end is obtained through the first collector, the posture information of the movable end is obtained through the second collector, and then the two are combined to obtain the pose of the movable end, which can combine the kinematics information of the robotic arm while positioning, which is helpful In order to reduce the influence of external noise on the positioning, the robustness and accuracy of the measurement results are improved.

In the embodiment of the present application, in step 203, the first error of the first pose information and the second error of the second pose information may be determined first, and then according to the first error, the second error, the first pose information and the The second pose information determines the pose information of the active end.

The specific process is shown in Figure 6:

Step 601: Determine the state transition matrix of the robotic arm through the motion information acquired by the first collector.

Step 602: Determine the input transfer matrix and the joint input matrix of the robot arm according to the control information corresponding to the motion information.

Step 603: Determine the current second pose information of the movable end according to the second pose information, the state transition matrix, the input transition matrix, and the joint input matrix of the movable end at the previous moment.

In the above scheme, by using the first error, the second error, the first pose information, and the second pose information to determine the pose information of the active end, not only the current pose of the active end can be predicted, but also the predictability of the predicted value can be calculated. The degree of trustworthiness helps to further improve the accuracy of positioning.

In the following, a specific example will be given for the solution of fusing the first pose information and the second pose information to obtain the final pose information P _{w of the movable end of the manipulator.}

Supposing the pose of the movable end of the i-1 _{s manipulator arm is P w(i-1)} , the predicted value of the pose of the movable end of _{the i s} manipulator arm can be obtained as:

P′ _w(i) =A·P _w(i-1) +B·u _i-1

Among them, A is the state transition matrix of the manipulator, which represents the function of calculating the position of the movable end from the joint angle, angular velocity and the parameters of the manipulator. B is the input transfer matrix of the mechanical arm, which represents the function of calculating the position of the movable end from the torque input by the motor. u _i-1 is the joint input matrix of the i-1 _s -th manipulator arm, which represents the input torque of each motor.

Then the prediction error covariance matrix of the end pose of the i _{s manipulator is:}

Among them, P _i-1 is the actual error covariance matrix of the end pose of the i-1 _s -th manipulator, and v is the measurement uncertainty caused by environmental noise or manipulator deformation.

Further, the system gain coefficient K _{i of the ith s} can be obtained by the following formula:

Among them, H is the observation matrix of the camera, representing the actual object, here is the function converted by the robot arm into image information, and R is the uncertainty of the camera measurement, representing the error calculated by the algorithm when evaluating the active end of the robot arm.

Therefore, the _{end pose of the i s-} th robotic arm is:

P _w(i) =P′ _w(i) +K _i ·(P″ _w(i) -H·P′ _w(i) )

Wherein, P _{"w (i)} as measured by the i-th camera position and orientation of the robot arm end _s.

From this, update the error covariance matrix for the next second as:

The above scheme predicts the current pose based on the second pose information of the active end at the last moment and the influence of the inertia of the robot arm at the last moment and the control information (such as the torque input by the motor) on the current motion at the last moment. Helps improve the accuracy of positioning.

Based on the same technical concept, FIG. 6 exemplarily shows the structure of the positioning device provided by the embodiment of the present invention, and the device can execute the flow of the positioning method.

As shown in Figure 7, the device may include:

The acquiring unit 701 is configured to acquire image information through the camera device of the robot, and acquire movement information through a collector provided on the robotic arm;

The processing unit 702 is configured to determine the first pose information of the movable end of the robot arm through the image information acquired by the camera device of the robot, and determine the motion information acquired by the collector provided on the robot arm. The second pose information of the movable end determines the pose information of the movable end according to the first pose information and the second pose information.

Optionally, the processing unit 702 is specifically configured to:

Obtain the movement information of each joint of the mechanical arm through the first collector, and determine the position information of the movable end according to the movement information of the joints and the position information of the base;

Acquiring the posture information of the movable end through the second collector;

According to the position information of the movable end and the posture information of the movable end, the second pose information of the movable end is determined.

Optionally, the processing unit 702 is specifically configured to: determine a first transformation matrix between the joints from the base to the movable end according to the angle information of the joints;

Determine the second coordinate of the movable end in the world coordinate system according to the first transformation matrix of each joint and the first coordinate of the base in the world coordinate system;

According to the position information of the movable end and the posture information of the movable end, determining the second pose information of the movable end includes:

Determine the rotation matrix of the movable end according to the posture information of the movable end;

Determining a second transformation matrix to transform from the base to the movable end according to the rotation matrix and the second coordinates;

Determine the second pose information of the movable end according to the second transformation matrix and the second coordinates.

Optionally, the processing unit 702 is specifically configured to determine the second coordinate of the movable end in the world coordinate system through formula (1):

Among them, [x _a , y _a , z _a , 1] are the coordinates of the base in the world coordinate system, and [x _h ′, y _h ′, z _h ′, 1] are the coordinates of the movable end of the robotic arm Coordinates in the world coordinate system, ^n-1 T _n is the first transformation matrix between the joints;

The second transformation matrix of the movable end is determined by formula (2):

Among them, R _h =R(θ _roll )R(θ _pitch )R(θ _yaw ) is the rotation matrix, θ _roll is the roll angle _{of the movable end of the robot arm, θ pitch} is the pitch angle of the movable end of the robot arm, θ _yaw is the yaw angle of the movable end of the robotic arm;

The second pose information of the movable end is determined by formula (3):

Formula (3) P _w ′ = ^w T _r ·P _r

Wherein, P _'W pose information of the second movable end, ^W T _r is a second movable end of the transformation matrix, P _r is the position of the base in the world coordinate system.

Optionally, the processing unit 702 is specifically configured to: determine the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm, including:

Determine the state transition matrix of the robotic arm through the motion information acquired by the first collector;

Determine the input transfer matrix and the joint input matrix of the robot arm according to the control information corresponding to the motion information;

Determine the current second pose information of the movable end according to the second pose information, the state transition matrix, the input transition matrix, and the joint input matrix of the movable end at the previous moment.

Optionally, the processing unit 702 is specifically configured to: determine the current second pose information of the active end through formula (4):

Formula (4) P′ _w(i) =A·P _w(i-1) +B·u _i-1

Wherein, A is the state transition matrix of the robotic arm, B is the input transition matrix of the robotic arm, and u _i-1 is the joint input matrix of the i-1sth robotic arm.

Optionally, the processing unit 702 is specifically configured to: the determining the pose information of the movable end according to the first pose information and the second pose information includes:

Determining the first error of the first pose information and the second error of the second pose information;

Determine the pose information of the movable end according to the first error, the second error, the first pose information, and the second pose information.

Optionally, the processing unit 702 is specifically configured to determine the pose information of the movable end through formula (5):

Formula (5) P _w(i) = P′ _w(i) + K _i ·(P″ _w(i) -H·P′ _w(i) )

P′ _w(i)为第二姿态信息，P″ _w(i)为第一姿态信息，K _i为增益系数，H为摄像装置的观测矩阵，

为第二位姿信息的第二误差，R为相机测量的不确定度。 among them,

P′ _w(i) is the second posture information, P″ _w(i) is the first posture information, K _i is the gain coefficient, H is the observation matrix of the camera device,

Is the second error of the second pose information, and R is the uncertainty of the camera measurement.

Optionally, the processing unit 702 is specifically configured to determine the second error of the second pose information through formula (6):

Among them, Pi _-1 is the pose of the movable end at the previous moment, and V is the environmental noise.

Based on the same technical concept, an embodiment of the present invention also provides a computing controlled device, including:

Memory, used to store program instructions;

The processor is configured to call the program instructions stored in the memory, and execute the above positioning method according to the obtained program.

Based on the same technical concept, embodiments of the present invention also provide a computer-readable non-volatile storage medium, including computer-readable instructions. When the computer reads and executes the computer-readable instructions, the computer is caused to perform the above positioning. Methods.

Finally, it should be noted that those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, optical storage, etc.) containing computer-usable program codes.

The present invention is described with reference to the flowchart and/or block diagram of the method, the controlled device (system), and the computer program product according to the present invention. It should be understood that each process and/or block in the flowchart and/or block diagram, and the combination of processes and/or blocks in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing controlled equipment to produce a machine, which can be executed by the processor of the computer or other programmable data processing controlled equipment The instructions generate means for realizing the functions specified in one process or multiple blocks in the flowchart and/or one block or multiple blocks in the block diagram.

These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing controlled equipment to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including the instruction device, The instruction device implements the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.

Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the scope of the present invention. In this way, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims (12)

A positioning method, characterized in that it is suitable for a robot provided with a mechanical arm, the mechanical arm being arranged on a base of the robot; the method includes: Determine the first pose information of the movable end of the robot arm through the image information acquired by the camera device of the robot; Determine the second pose information of the movable end through the movement information acquired by the collector provided on the robotic arm; Determine the pose information of the movable end according to the first pose information and the second pose information. The method according to claim 1, wherein the determining the second pose information of the movable end through the movement information obtained by the collector provided on the mechanical arm comprises: Obtain the movement information of each joint of the mechanical arm through the first collector, and determine the position information of the movable end according to the movement information of the joints and the position information of the base; Acquiring the posture information of the movable end through the second collector; According to the position information of the movable end and the posture information of the movable end, the second pose information of the movable end is determined. The method according to claim 2, wherein the determining the position information of the movable end according to the movement information of the joints and the position information of the base comprises: Determine the first transformation matrix between the joints from the base to the movable end according to the angle information of the joints; Determine the second coordinate of the movable end in the world coordinate system according to the first transformation matrix of each joint and the first coordinate of the base in the world coordinate system; According to the position information of the movable end and the posture information of the movable end, determining the second pose information of the movable end includes: Determine the rotation matrix of the movable end according to the posture information of the movable end; Determining a second transformation matrix to transform from the base to the movable end according to the rotation matrix and the second coordinates; Determine the second pose information of the movable end according to the second transformation matrix and the second coordinates. The method according to claim 3, wherein the second coordinate of the movable end in the world coordinate system is determined by formula (1):

Among them, [x _a , y _a , z _a , 1] are the coordinates of the base in the world coordinate system, and [x _h ′, y _h ′, z _h ′, 1] are the coordinates of the movable end of the robotic arm Coordinates in the world coordinate system, ^n-1 T _n is the first transformation matrix between the joints; The second transformation matrix of the movable end is determined by formula (2):

Among them, R _h =R(θ _roll )R(θ _pitch )R(θ _yaw ) is the rotation matrix, θ _roll is the roll angle _{of the movable end of the robot arm, θ pitch} is the pitch angle of the movable end of the robot arm, θ _yaw is the yaw angle of the movable end of the robotic arm; The second pose information of the movable end is determined by formula (3): Formula (3) P _w ′ = ^w T _r ·P _r Wherein, P _'W pose information of the second movable end, ^W T _r is a second movable end of the transformation matrix, P _r is the position of the base in the world coordinate system. The method according to claim 1, wherein the determining the second pose information of the movable end through the movement information obtained by the collector provided on the mechanical arm comprises: Determine the state transition matrix of the robotic arm through the motion information acquired by the first collector; Determining the input transfer matrix and the joint input matrix of the robot arm according to the control information corresponding to the motion information; Determine the current second pose information of the movable end according to the second pose information, the state transition matrix, the input transition matrix, and the joint input matrix of the movable end at the previous moment. The method according to claim 5, wherein the current second pose information of the active end is determined by formula (4) Formula (4) P′ _w(i) =A·P _w(i-1) +B·u _i-1 Wherein, A is the state transition matrix of the robotic arm, B is the input transition matrix of the robotic arm, and u _i-1 is the joint input matrix of the i-1sth robotic arm. The method according to any one of claims 1 to 6, wherein the determining the pose information of the movable end according to the first pose information and the second pose information comprises: Determining the first error of the first pose information and the second error of the second pose information; Determine the pose information of the movable end according to the first error, the second error, the first pose information, and the second pose information. The method according to claim 7, characterized in that the pose information of the movable end is determined by formula (5): Formula (5) P _w(i) = P′ _w(i) + K _i ·(P″ _w(i) -H·P′ _w(i) ) among them,

P′ _w(i) is the second posture information, P″ _w(i) is the first posture information, K _i is the gain coefficient, H is the observation matrix of the camera device,

Is the second error of the second pose information, and R is the uncertainty of the camera measurement. The method according to claim 7, wherein the second error of the second pose information is determined by formula (6):

Among them, Pi _-1 is the pose of the movable end at the previous moment, and V is the environmental noise. A positioning device, characterized in that it is suitable for a robot provided with a robotic arm, the robotic arm is arranged on a base of the robot, and the device comprises: An acquiring unit, configured to acquire image information through the camera device of the robot, and acquire movement information through a collector provided on the robotic arm; The processing unit is used to determine the first posture information of the movable end of the robot arm through the image information acquired by the camera device of the robot, and determine the activity through the motion information acquired by the collector provided on the robot arm The second pose information of the mobile terminal determines the pose information of the movable terminal according to the first pose information and the second pose information. A computing device, characterized in that it comprises: Memory, used to store program instructions; The processor is configured to call the program instructions stored in the memory, and execute the method according to any one of claims 1 to 9 according to the obtained program. A computer-readable non-volatile storage medium, characterized by comprising computer-readable instructions, when the computer reads and executes the computer-readable instructions, the computer is caused to execute any one of claims 1 to 9 Methods.

Images