CN117213475A - Data processing method and device for bipedal robot and storage medium - Google Patents
Data processing method and device for bipedal robot and storage medium Download PDFInfo
- Publication number
- CN117213475A CN117213475A CN202311013119.8A CN202311013119A CN117213475A CN 117213475 A CN117213475 A CN 117213475A CN 202311013119 A CN202311013119 A CN 202311013119A CN 117213475 A CN117213475 A CN 117213475A
- Authority
- CN
- China
- Prior art keywords
- data
- moment
- pose
- camera
- kinematic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000003672 processing method Methods 0.000 title claims abstract description 35
- 230000033001 locomotion Effects 0.000 claims abstract description 331
- 238000005259 measurement Methods 0.000 claims abstract description 156
- 238000004364 calculation method Methods 0.000 claims abstract description 63
- 238000006243 chemical reaction Methods 0.000 claims description 130
- 238000012545 processing Methods 0.000 claims description 99
- 210000003141 lower extremity Anatomy 0.000 claims description 86
- 230000008859 change Effects 0.000 claims description 80
- 238000000034 method Methods 0.000 claims description 63
- 230000009466 transformation Effects 0.000 claims description 48
- 238000006073 displacement reaction Methods 0.000 claims description 38
- 230000000007 visual effect Effects 0.000 claims description 31
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 20
- 230000017105 transposition Effects 0.000 claims description 20
- 238000013507 mapping Methods 0.000 claims description 14
- 238000000605 extraction Methods 0.000 claims description 13
- 238000005457 optimization Methods 0.000 claims description 13
- 206010034719 Personality change Diseases 0.000 claims description 6
- 230000008569 process Effects 0.000 description 30
- 238000010586 diagram Methods 0.000 description 22
- 230000036544 posture Effects 0.000 description 15
- 238000005516 engineering process Methods 0.000 description 13
- 238000013473 artificial intelligence Methods 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 12
- 238000004891 communication Methods 0.000 description 12
- 230000006870 function Effects 0.000 description 10
- 230000010354 integration Effects 0.000 description 10
- 230000008878 coupling Effects 0.000 description 9
- 238000010168 coupling process Methods 0.000 description 9
- 238000005859 coupling reaction Methods 0.000 description 9
- 238000001514 detection method Methods 0.000 description 9
- 230000005021 gait Effects 0.000 description 9
- 230000001133 acceleration Effects 0.000 description 8
- 238000010276 construction Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 4
- 210000002414 leg Anatomy 0.000 description 4
- 230000001360 synchronised effect Effects 0.000 description 4
- 238000012549 training Methods 0.000 description 4
- 239000013598 vector Substances 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 239000000470 constituent Substances 0.000 description 2
- 210000003127 knee Anatomy 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000009012 visual motion Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 230000037147 athletic performance Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 210000004705 lumbosacral region Anatomy 0.000 description 1
- 238000003058 natural language processing Methods 0.000 description 1
- 235000021018 plums Nutrition 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
The application discloses a data processing method, a device and a storage medium for a bipedal robot, which are used for acquiring kinematic data of the bipedal robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment, and acquiring a last moment image and a current moment image which are shot by a camera; then, acquiring the initial pose of the camera at the current moment; then, according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose, the kinematic residual error of the camera is obtained through calculation, and then according to the kinematic residual error, the inertial measurement data, the image at the previous moment and the image at the current moment, the target pose of the camera at the current moment is obtained through calculation. The embodiment of the application can reduce the influence of image motion blur and improve the robustness of pose estimation, thereby obtaining more accurate pose information.
Description
Technical Field
The present application relates to the field of artificial intelligence technologies, and in particular, to a data processing method and apparatus for a bipedal robot, and a storage medium.
Background
The vision-based synchronous positioning and mapping (Visual Simultaneous Localization And Mapping, VSLAM) plays a role in weight lifting in the task execution process of the mobile robot, for example, the mobile robot can identify the relative position and direction of an obstacle through an image sensor, so that the body is prevented from being in contact with the object; and the global consistency of the map constructed by the mobile robot and the scene environment is ensured by sensing and estimating the space pose information of the body.
At present, because the image sensor of biped robot sets up in the head, and biped robot's biped motion mode leads to the motion blur to appear in the image acquisition in-process easily to reduce the image matching effect, reduce the stability of VSLAM in-process discernment simultaneously, disturb the position appearance and estimate, lead to biped robot's space position appearance information's accuracy to be poor.
Disclosure of Invention
The following is a summary of the subject matter described in detail herein. This summary is not intended to limit the scope of the claims.
The embodiment of the application provides a data processing method, a data processing device and a storage medium for a bipedal robot, which can reduce the influence of image motion blur and improve the robustness of pose estimation, so that more accurate pose information can be obtained.
In one aspect, an embodiment of the present application provides a data processing method for a bipedal robot, a head position of the bipedal robot being provided with a camera, the method including the steps of:
acquiring kinematic data of the biped robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment;
acquiring a last moment image and a current moment image which are shot by the camera;
acquiring an initial pose of the camera at the current moment;
calculating to obtain a kinematic residual error of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose;
and calculating the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image.
On the other hand, the embodiment of the application also provides a data processing device for a bipedal robot, wherein a camera is arranged at the head position of the bipedal robot, and the data processing device comprises:
the data acquisition unit is used for acquiring kinematic data of the bipedal robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment;
The image acquisition unit is used for acquiring a last moment image and a current moment image which are shot by the camera;
the pose acquisition unit is used for acquiring the initial pose of the camera at the current moment;
the residual calculation unit is used for calculating the kinematic residual of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose;
and the pose calculating unit is used for calculating the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image.
Optionally, the residual calculation unit is further configured to:
according to the kinematic data at the previous moment and the kinematic data at the current moment, calculating to obtain motion increment data of the camera;
and calculating to obtain the kinematic residual error of the camera according to the motion increment data and the initial pose.
Optionally, the residual calculation unit is further configured to:
acquiring a historical pose of the camera at the last moment;
calculating to obtain the pose variation of the camera according to the historical pose and the initial pose;
and calculating to obtain the kinematic residual error of the camera according to the motion increment data and the pose variation.
Optionally, the residual calculation unit is further configured to:
performing difference calculation on the displacement increment data and the displacement change data to obtain a displacement change value;
carrying out logarithmic mapping according to the rotation increment data and the rotation change data to obtain an attitude change value;
and constructing and obtaining the kinematic residual error of the camera according to the displacement variation value and the attitude variation value.
Optionally, the residual calculation unit is further configured to:
determining a supporting foot at the previous moment according to the foot stress data at the previous moment, and determining a supporting foot at the current moment according to the foot stress data at the current moment;
according to a first motion chain from the support foot at the previous moment to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the previous moment to obtain lower limb conversion data at the previous moment;
according to a second motion chain from the supporting foot at the current moment to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the current moment to obtain lower limb conversion data at the current moment;
according to a third motion chain from the waist of the bipedal robot to the camera, respectively performing data conversion on the trunk motion data at the last moment and the trunk motion data at the current moment to obtain trunk conversion data at the last moment and trunk conversion data at the current moment;
And calculating to obtain the motion increment data of the camera according to the lower limb conversion data at the last moment, the lower limb conversion data at the current moment, the trunk conversion data at the last moment and the trunk conversion data at the current moment.
Optionally, the residual calculation unit is further configured to:
determining foot pose transformation data according to the last moment supporting foot and the current moment supporting foot data;
performing matrix transposition according to the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment to obtain pose transposition data at the previous moment;
multiplying the bipedal pose transformation data, the pose transposition data at the last moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain motion increment data of the camera.
Optionally, the pose acquisition unit is further configured to:
integrating the inertial measurement data to obtain initial waist pose data;
according to a fourth motion chain from the waist of the biped robot to the camera, performing data conversion on the trunk motion data at the current moment to obtain trunk initial conversion data;
and calculating to obtain the initial pose of the camera at the current moment according to the initial pose data of the waist and the initial conversion data of the trunk.
Optionally, the pose acquisition unit is further configured to:
and predicting the pose of the lower limb movement data at the current moment and the trunk movement data at the current moment to obtain the initial pose of the camera at the current moment.
Optionally, the pose calculating unit is further configured to:
pre-integrating the inertial measurement data to obtain inertial data residual errors;
performing feature extraction and feature matching processing on the previous moment image and the current moment image to obtain a visual re-projection error;
and calculating to obtain the target pose of the camera at the current moment according to the kinematic residual error, the inertial data residual error and the visual re-projection error.
Optionally, the data processing device for a bipedal robot further comprises:
and the secondary residual calculation unit is used for calculating the kinematic secondary residual of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the target pose if the current moment image is confirmed to be a key image.
The historical information acquisition unit is used for acquiring historical key images and historical inertial measurement data of corresponding moments of the historical key images;
And the data local optimization unit is used for carrying out optimization processing on the map points corresponding to the target pose and the current moment image according to the kinematic secondary residual error, the inertia measurement data, the historical key image and the current moment image to obtain the optimized key pose and key map points.
Optionally, the secondary residual calculation unit is further configured to:
acquiring a historical key pose at a moment corresponding to the historical key image, and calculating to obtain a key pose conversion amount of the camera according to the historical key pose and the target pose;
calculating key motion increment data of the camera according to the kinematic data at the previous moment and the kinematic data at the current moment;
and calculating to obtain a kinematic secondary residual error of the camera according to the key motion increment data and the key pose transformation quantity.
On the other hand, the embodiment of the application also provides a data processing device for the bipedal robot, which comprises:
at least one processor;
at least one memory for storing at least one program;
the data processing method for a bipedal robot as described above is implemented when at least one of the programs is executed by at least one of the processors.
In another aspect, embodiments of the present application also provide a computer readable storage medium in which a computer program executable by a processor is stored, the computer program executable by the processor being configured to implement a data processing method for a bipedal robot as described above.
In another aspect, an embodiment of the present application further provides a computer program product, including a computer program or computer instructions stored in a computer readable storage medium, from which a processor of a data processing device for a bipedal robot reads, the processor executing the computer program or the computer instructions, so that the data processing device for a bipedal robot performs the data processing method for a bipedal robot as described above.
The embodiment of the application at least comprises the following beneficial effects: after the kinematic data of the biped robot at the last moment, the kinematic data of the current moment, the inertial measurement data from the last moment to the current moment and the initial pose of the current moment are obtained, the kinematic residual error of the camera can be obtained through calculation according to the kinematic data of the last moment, the kinematic data of the current moment and the initial pose, and the biped gait of the biped robot has corresponding swing tracks and impact force generated by the feet striking the ground, so that the image motion blur is easy to be caused, and the deviation of the camera pose between the data calculated by the kinematic data (namely the actual observation transformation quantity) and the data calculated by the initial pose (namely the estimated transformation quantity) is determined through the change of the motion state of the biped robot at the last moment and the current moment, thereby being beneficial to correcting the camera pose and reducing the influence of the image motion blur. In addition, a last moment image and a current moment image which are shot by a camera can be obtained, then a target pose of the camera at the current moment can be obtained through calculation according to a kinematic residual error, inertial measurement data from the last moment to the current moment, the last moment image and the current moment image, and the initial pose is optimized and adjusted by combining the motion relation between the inertial measurement data and the kinematic data of a plurality of moments and the visual projection relation between the last moment image and the current moment image, so that tight coupling of multi-source information can be realized, robustness of pose estimation can be improved, and more accurate pose data of the bipedal robot can be obtained.
Additional features and advantages of the application will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the application. The objectives and other advantages of the application may be realized and attained by the structure particularly pointed out in the written description and drawings.
Drawings
The accompanying drawings are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate and do not limit the application.
FIG. 1 is a schematic illustration of an implementation environment provided by an embodiment of the present application;
FIG. 2 is a schematic illustration of another implementation environment provided by an embodiment of the present application;
FIG. 3 is a flow chart of a data processing method for a bipedal robot provided by an embodiment of the application;
fig. 4 is a schematic structural view of a bipedal robot according to an embodiment of the present application;
FIG. 5 is a schematic diagram of an initial pose calculation method according to an embodiment of the present application;
FIG. 6 is a schematic diagram of motion state prediction provided by an embodiment of the present application;
FIG. 7 is a schematic diagram of a motion delta data calculation mode according to an embodiment of the present application;
FIG. 8 is a schematic diagram of calculating kinematic residuals provided by an embodiment of the present application;
FIG. 9 is a flow chart of a data processing method in the related art;
FIG. 10 is a schematic illustration of a pose change provided by an embodiment of the present application;
FIG. 11 is a schematic diagram of a bipedal robot in different support states according to an embodiment of the application;
fig. 12 is a schematic diagram of kinematic data calculation of a bipedal robot provided by an embodiment of the present application;
fig. 13 is a schematic diagram of a conversion relationship of a motion coordinate system of a biped robot according to an embodiment of the present application;
fig. 14 is a schematic diagram of an actual application scenario of a data processing method for a bipedal robot according to an embodiment of the present application;
FIG. 15 is an overall flow chart of a data processing method for a bipedal robot provided by one specific example of the present application;
FIG. 16 is a schematic diagram of a data processing apparatus for a bipedal robot provided in an embodiment of the application;
fig. 17 is a schematic view of another data processing apparatus for a bipedal robot according to an embodiment of the application.
Detailed Description
The application will be further described with reference to the drawings and specific examples. The described embodiments should not be taken as limitations of the present application, and all other embodiments that would be obvious to one of ordinary skill in the art without making any inventive effort are intended to be within the scope of the present application.
In the following description, reference is made to "some embodiments" which describe a subset of all possible embodiments, but it is to be understood that "some embodiments" can be the same subset or different subsets of all possible embodiments and can be combined with one another without conflict.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of the application only and is not intended to be limiting of the application.
Before describing embodiments of the present application in further detail, the terms and terminology involved in the embodiments of the present application will be described, and the terms and terminology involved in the embodiments of the present application will be used in the following explanation.
1) Computer Vision (CV) is a science of researching how to make a machine "look at", and more specifically, it means to replace a human eye with a camera and a Computer to perform machine Vision such as identifying and measuring on a target, and further perform graphic processing, so that the Computer is processed into an image more suitable for human eye observation or transmitting to an instrument for detection. As a scientific discipline, computer vision research-related theory and technology has attempted to build artificial intelligence systems that can acquire information from images or multidimensional data. Computer vision techniques typically include image processing, image recognition, image semantic understanding, image retrieval, OCR, video processing, video semantic understanding, video content/behavior recognition, three-dimensional object reconstruction, 3D techniques, virtual reality, augmented reality, synchronous positioning, and map construction, among others, as well as common biometric recognition techniques such as face recognition, fingerprint recognition, and others.
2) Artificial intelligence (Artificial Intelligence, AI) is the theory, method, technique and application system that uses a digital computer or a machine controlled by a digital computer to simulate, extend and extend human intelligence, sense the environment, acquire knowledge and use the knowledge to obtain optimal results. In other words, artificial intelligence is an integrated technology of computer science that attempts to understand the essence of intelligence and to produce a new intelligent machine that can react in a similar way to human intelligence. Artificial intelligence, i.e. research on design principles and implementation methods of various intelligent machines, enables the machines to have functions of sensing, reasoning and decision. The artificial intelligence technology is a comprehensive subject, and relates to the technology with wide fields, namely the technology with a hardware level and the technology with a software level. Artificial intelligence infrastructure technologies generally include, for example, sensors, dedicated artificial intelligence chips, cloud computing, distributed storage, big data processing technologies, pre-training model technologies, operation/interaction systems, mechatronics, and the like. The pre-training model is also called a large model and a basic model, and can be widely applied to all large-direction downstream tasks of artificial intelligence after fine adjustment. The artificial intelligence software technology mainly comprises a computer vision technology, a voice processing technology, a natural language processing technology, machine learning/deep learning and other directions.
3) The vision-based synchronous positioning and mapping (Visual Simulatneous Localization And Mapping, VSLAM) refers to the process that a moving object is positioned and mapped according to information of sensors, including vision sensors such as cameras, by acquiring environment image information, calculating the position of the moving object by image processing and constructing an environment map, and the problem of positioning and mapping when a robot and the like move in an unknown environment is solved. VSLAM can be applied to robots, the uses of which include the positioning of the sensors themselves, and subsequent path planning, athletic performance, scene understanding.
4) Pose, which is to describe the position and the pose of an object (such as a coordinate) under a specified coordinate system, such as a common pose to describe the position and the pose of a robot in a space coordinate system; wherein, the position refers to the positioning of the rigid body in space, and the position of the rigid body can be represented by a matrix of 3×1, namely, the position of the center of the rigid body coordinate system in the base coordinate system; the posture refers to the orientation of the rigid body in space, and the posture of the rigid body can be represented by a 3×3 matrix, namely, the posture of the rigid body coordinate system in the base coordinate system.
At present, vision-based synchronous positioning and map construction play a role in lifting weight in the process of executing tasks by a mobile robot, for example, the mobile robot can identify the relative position and direction of an obstacle through a vision sensor, so that the body is prevented from being in contact with and collision with the object; and the global consistency of the map constructed by the mobile robot and the scene environment is ensured by sensing and estimating the space pose information of the body.
Because the image sensor of the biped robot is arranged on the head, and the biped motion mode of the biped robot is different from the motion mode of a wheeled or crawler type mobile robot in the related art, the swing track designated by the biped gait and the ground impact reaction force generated by the foot striking the ground are easy to generate image motion blur in the process of acquiring images by the image sensor. In addition, as the biped motion mode of the biped robot has poor stability compared with the multi-foot motion mode of the biped robot with three feet or more, the shaking amplitude of the image sensor of the head is larger, so that the biped robot is easier to cause image motion blurring in the image acquisition process, the image matching effect of the biped robot is reduced, meanwhile, the stability of identification in the VSLAM process is reduced, the pose estimation is disturbed, and the accuracy of the spatial pose information of the biped robot is poor.
In order to obtain more accurate pose of the biped robot, improve and improve the image matching effect, reduce the influence of image motion blur on the image matching effect, improve the accuracy of a VSLAM system building map, the embodiment of the application provides a data processing method for the biped robot, a data processing device for the biped robot, a computer-readable storage medium and a computer program product, firstly, the last moment kinematic data of the biped robot, the current moment kinematic data and the inertial measurement data from the last moment to the current moment are obtained, the last moment image and the current moment image which are shot by a camera arranged at the head position of the biped robot can be obtained, and in addition, the initial pose of the camera at the current moment can be obtained, so that the kinematic residual error of the camera can be calculated according to the last moment kinematic data, the current moment kinematic data and the initial pose, therefore, the motion change of the biped robot at the adjacent moment can be calculated, the deviation between the calculated data (namely the actual conversion quantity) of the motion data and the initial pose calculated (namely the actual conversion quantity) can be determined, and the motion pose is further, the influence of the motion of the camera can be corrected is reduced. Then, according to the kinematic residual error, the inertial measurement data from the last moment to the current moment, the last moment image and the current moment image, the target pose of the camera at the current moment is obtained through calculation, and the initial pose is optimized and adjusted by combining the motion relation between the inertial measurement data and the kinematic data at a plurality of moments and the visual projection relation between the last moment image and the current moment image, so that the tight coupling of multi-source information can be realized, the robustness of pose estimation can be improved, and more accurate pose data of the bipedal robot can be obtained.
Referring to fig. 1, fig. 1 is a schematic diagram of an implementation environment provided by an embodiment of the present application. The implementation environment may include the first robot terminal 110 and the data processing server 120, and the first robot terminal 110 and the data processing server 120 may be directly or indirectly connected through wired or wireless communication. The first robot terminal 110 and the data processing server 120 may be one node server in a blockchain, which is not specifically limited in this embodiment.
The first robot terminal 110 may be a smart phone, a tablet computer, a notebook computer, a desktop computer, a smart speaker, a smart watch, a vehicle-mounted terminal, etc., but is not limited thereto. The first robot terminal 110 may be directly or indirectly connected to a kinematic sensor such as a joint encoder or a pressure sensor provided in the bipedal robot to measure kinematic data of the bipedal robot, an inertial measurement unit provided at a waist position of the bipedal robot, and a camera provided at a head position of the bipedal robot through wired or wireless communication, so that the kinematic data, the inertial measurement data, and an image captured by the camera of the bipedal robot may be received.
The data processing server 120 may be an independent physical server, a server cluster or a distributed system formed by a plurality of physical servers, or a cloud server providing cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, CDNs (Content Delivery Network, content delivery networks), basic cloud computing services such as big data and artificial intelligence platforms, and the like. The data processing server 120 can obtain an initial pose of the camera at the current moment, and calculate to obtain a kinematic residual error of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose; and the target pose of the camera at the current moment can be calculated according to the kinematic residual error, the inertial measurement data from the last moment to the current moment, the last moment image and the current moment image.
Referring to fig. 1, in an application scenario, it is assumed that the first robot terminal 110 is a computer, and the first robot terminal 110 may be connected to a joint encoder, an inertial measurement unit, and a camera mounted on a bipedal robot by wireless communication. The first robot terminal 110 may be used as a data collector of the biped robot, obtain the kinematic data recorded and measured by the joint encoders of the biped robot at each moment in the motion process, the inertial measurement data measured by the inertial measurement unit, and the images obtained by camera shooting, the first robot terminal 110 may send the kinematic data of the biped robot at the last moment, the kinematic data of the current moment, the inertial measurement data of the last moment to the current moment, and the images of the last moment obtained by camera shooting to the data processing server 120, at this time, the data processing server 120 may obtain the initial pose of the biped robot at the current moment by the data sent by the first robot terminal 110, or may obtain the initial pose of the biped robot at the current moment by using the pose prediction model, then, the data processing server 120 may calculate the kinematic residual of the camera according to the kinematic data of the last moment, the kinematic data of the current moment and the initial pose, and the previous moment, and the images of the current moment, and then calculate the pose of the camera to obtain the map data of the current robot at the current moment, and then the current position of the terminal 110 may be used to make the map to be displayed by the first robot terminal 110, and then the map the current terminal may be processed.
Referring to fig. 2, fig. 2 is a schematic diagram of another implementation environment provided by an embodiment of the present application. The implementation environment may include a second robot terminal 210, a first processing server 220, and a second processing server 230, and the first processing server 220 may be directly or indirectly connected to the second processing server 230 and the second robot terminal 210 through wired or wireless communication, respectively. The second robot terminal 210, the first processing server 220, and the second processing server 230 may be nodes in a blockchain, which is not specifically limited in this embodiment.
The second robot terminal 210 may be a smart phone, a tablet computer, a notebook computer, a desktop computer, a smart speaker, a smart watch, a car terminal, etc., but is not limited thereto. The second robot terminal 110 may be directly or indirectly connected to a kinematic sensor provided in the bipedal robot, such as a joint encoder or a pressure sensor, for measuring kinematic data of the bipedal robot, an inertial measurement unit provided at a lumbar region of the bipedal robot, and a camera provided at a head region of the bipedal robot by a wired or wireless communication method, so that the kinematic data, the inertial measurement data, and an image captured by the camera of the bipedal robot may be received.
The first processing server 220 and the second processing server 230 may be independent physical servers, may be a server cluster or a distributed system formed by a plurality of physical servers, and may also be cloud servers for providing cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, CDNs (Content Delivery Network, content delivery networks), basic cloud computing services such as big data and artificial intelligence platforms, and the like. The second processing server 230 may receive the initial pose of the camera at the current moment, the previous moment kinematic data of the biped robot, and the current moment kinematic data sent by the first processing server 220, calculate to obtain a kinematic residual of the camera according to the previous moment kinematic data, the current moment kinematic data, and the initial pose, and then return the kinematic residual to the first processing server 220; the first processing server 220 may obtain the initial pose of the camera at the current time, and send the initial pose of the camera at the current time to the second processing server 230, or calculate the target pose of the camera at the current time by using the kinematic residual error sent by the second processing server 230, the inertial measurement data from the last time to the current time sent by the second robot terminal 210, the last time image, and the current time image.
Referring to fig. 2, in another application scenario, it is assumed that the second robot terminal 210 is a computer, and the second robot terminal 210 may be connected with a joint encoder, an inertial measurement unit, and a camera mounted to the bipedal robot by wireless communication. The second robot terminal 210 may be used as a data collector of the biped robot to obtain the kinematic data recorded and measured by the joint encoders of the biped robot at each moment in the motion process, the inertial measurement data measured by the inertial measurement unit, and the image obtained by camera shooting, the second robot terminal 210 may send the kinematic data of the biped robot at the last moment, the kinematic data of the current moment, the inertial measurement data of the last moment to the current moment, and the image obtained by camera shooting to the first processing server 220, at this time, the first processing server 220 may obtain the initial pose of the biped robot at the current moment by the data sent by the second robot terminal 210, and may also obtain the initial pose of the biped robot at the current moment by predicting the pose by using the pose prediction model, then the first processing server 220 may send the initial pose to the second processing server 230, and the second processing server 230 may obtain the kinematic data of the last moment and the kinematic data of the current moment by the second robot terminal 210, so that the first processing server 230 may calculate the initial pose and the motion residual error according to the kinematic data of the first moment and the pose of the current moment; alternatively, the first processing server 220 may transmit the initial pose to the second processing server 230 and simultaneously transmit the previous time kinematic data and the current time kinematic data from the second robot terminal 210 to the second processing server 230, so that the second processing server 230 may calculate the kinematic residual of the camera according to the previous time kinematic data, the current time kinematic data and the initial pose. Then, the second processing server 230 may return the kinematic residual error of the camera to the first processing server 220, and then the first processing server 220 may calculate the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data from the previous moment to the current moment, the previous moment image and the current moment image, so that the first processing server 220 may perform positioning and map construction of the bipedal robot by using the target pose at the current moment and the current moment image, and then the first processing server 220 may return the constructed map to the second robot terminal 210, so that the second robot terminal 210 may display the constructed map.
Fig. 3 is a flowchart of a data processing method for a bipedal robot, which may be performed by a server, according to an embodiment of the present application. Referring to fig. 3, the data processing method for the bipedal robot may include, but is not limited to, steps 310 to 350.
Step 310: and acquiring kinematic data of the biped robot at the last moment, kinematic data of the biped robot at the current moment and inertial measurement data from the last moment to the current moment.
In an embodiment, the kinematic data of the bipedal robot may refer to data for describing the motion of the bipedal robot, and since the bipedal robot mainly relies on joints to control the relative motion between its constituent parts, for example, the mechanical legs of the bipedal robot may be connected to the waist of the bipedal robot through the joints, so that the mechanical legs of the bipedal robot may rotate relative to the waist under the driving of the driving device, so that the mechanical legs and the waist generate relative motion, and further the movement of the bipedal robot is realized. Therefore, the motion of the biped robot can be controlled by controlling the joints in the biped robot to complete corresponding motions, in order to accurately control the motions and positions of the joints, the rotating speeds of the two output shafts of the driving device are required to be accurately measured, the rotating speeds of the two output shafts of the driving device are usually measured by adopting an encoder, the rotating speed signals of the two output shafts are converted into sine waves through a magnetic or optical measurement principle, and the motion gesture of the biped robot can be read through acquiring the encoded data of the joint encoder, so that the kinematic data of the biped robot can refer to the encoded data of the joint encoder corresponding to the joints used for controlling the motions of the biped robot in the biped robot, and the relative positions of the parts of the biped robot can be determined by utilizing the kinematic data of the biped robot, and the motion gesture of the biped robot can be further determined. The joint encoder in the biped robot can be directly or indirectly connected with the robot terminal in a wired or wireless communication mode, so that the robot terminal can acquire the kinematic data of the biped robot.
In an embodiment, the kinematic data of the bipedal robot may be stress data, displacement data, speed data, etc. of the constituent parts of the bipedal robot in addition to the encoded data of the joint encoder, and the corresponding measuring device may be provided to a pressure sensor, a linear motion sensor, a speed sensor, etc. of the bipedal robot, which is not particularly limited in the embodiment of the present application.
In an embodiment, the inertial measurement data may refer to data describing three-axis attitude angle, speed and acceleration of the bipedal robot in a space coordinate system, the inertial measurement data may be used to describe the spatial attitude of the bipedal robot, where the inertial measurement data may be obtained by measuring by an inertial measurement unit (Inertial Measurement Unit, IMU), the inertial measurement unit is a device for measuring the three-axis attitude angle (or angular velocity) and acceleration of an object, the inertial measurement unit generally includes three single-axis accelerometers and gyroscopes corresponding to the three single axes, the accelerometers may detect acceleration signals of the object on the independent three axes of the carrier coordinate system, the gyroscopes may detect angular velocity signals of the carrier relative to the navigation coordinate system, and the inertial measurement unit of the bipedal robot may be disposed at a waist position, so that the angular velocity and acceleration of the bipedal robot in a three-dimensional space centered on the waist position may be measured by the inertial measurement unit, and the attitude of the bipedal robot may be calculated by using the inertial measurement data. It should be noted that, the inertial measurement unit may be directly or indirectly connected to the robot terminal through wired or wireless communication, so that the robot terminal may acquire inertial measurement data of the bipedal robot.
Referring to fig. 4, fig. 4 is a schematic structural diagram of a bipedal robot according to an embodiment of the application. It can be seen that the bipedal robot is provided with a plurality of joint encoders, such as joint encoders provided at the head, waist 410, crotch, knees and feet, which can measure the motions and positions of the respective joints, and the kinematic data measured by the joint encoders can make the positions of the respective joints through specific coordinates in a coordinate system established centering on the joint encoders, and the motions of the respective joints can be represented by specific vectors in the same coordinate system. Specifically, as shown in the left foot 420 of the biped robot of fig. 4, the joint encoder located at the left foot 420 may measure the rotation angle of the joint of the left foot 420, such as the rotation angle of the X axis, and the rotation angle of the X axis with respect to the Y axis, and thus may calculate the position of the left foot 420 and the position of the knee of the left leg through the length of the link of the joint of the left foot 420. Therefore, the position and the posture of each joint part of the biped robot can be judged by utilizing the kinematic data of each joint part of the biped robot, so that the motion state of the biped robot can be judged, and more accurate camera postures can be calculated.
Step 320: and acquiring the last moment image and the current moment image shot by the camera.
In an embodiment, a camera is arranged on the head of the biped robot, and the camera can be used for shooting to obtain an environment image of the current environment of the biped robot. Therefore, a road mark point map of the surrounding environment can be established through the images shot by the cameras arranged on the heads of the bipedal robots, namely the vision sensors, so that the positions of the bipedal robots relative to the road mark points can be determined, and the surrounding environment of the bipedal robots can be reconstructed.
The camera arranged on the head of the biped robot can be at least one of a monocular camera, a binocular camera or a depth image camera.
In an embodiment, the time interval, that is, the frequency of the image shooting by the camera may be equal to the measurement frequency of the kinematic data and the inertial measurement data of the biped robot, so that it may be determined that the corresponding relationship between the kinematic data, the inertial measurement data and the image is established in the time dimension, and then the kinematic data, the inertial measurement data and the image at the same moment may be determined according to the corresponding relationship between the kinematic data, the inertial measurement data and the image, so that errors caused by misalignment of information in the time dimension are reduced, and the accuracy of pose estimation is facilitated to be improved. In addition, the real-time video stream data can be shot through the camera and transmitted to the robot terminal, and the robot terminal can extract frames of the real-time video stream data according to the sampling frequency identical to that of the kinematic data, so that images at corresponding moments are obtained.
Step 330: and acquiring the initial pose of the camera at the current moment.
In an embodiment, the initial pose of the camera at the current moment may refer to a pose of estimating and predicting the camera at the current moment, the initial pose may represent an initial position and an initial pose of the camera in a three-dimensional space, and motion of the camera may be estimated by using the initial poses of the camera at a plurality of continuous moments, so as to help optimize the pose of the camera and improve accuracy of estimating the pose of the camera. The initial pose of the camera at the current moment can be estimated through a motion state of a last camera, and also can be estimated through global repositioning, for example, after the pose of the camera is successfully acquired at the last moment, the camera can be assumed to perform constant-speed motion from the last moment to the current moment, so that the initial pose of the current moment can be estimated by using a constant-speed motion model, specifically, the initial pose of the camera at the current moment can be estimated by using the historical pose of the camera at the last moment and the displacement speed of the camera at the last moment, wherein the pose of the last camera can refer to the historical pose of the camera at the last moment, and also can refer to the historical key pose corresponding to the last key image; and when the number of feature matching points obtained after feature extraction and feature matching processing is smaller than a preset threshold value, the image identification failure at the current moment is indicated, the initial pose at the current moment can be estimated through global repositioning, specifically, the map point corresponding to the last key image can be utilized for iterative optimization, the predicted map point of the image at the current moment is obtained through prediction, and the initial pose is estimated according to the predicted map point.
In an embodiment, the estimation process of the initial pose may be performed in a robot terminal or may be performed in a server, when the robot terminal performs the estimation process of the initial pose, the robot terminal may perform estimation of the initial pose of the camera at the current moment by using the observation data (such as kinematic data, inertial measurement data, images, etc.) after receiving the observation data transmitted by each sensor (including the kinematic sensor, the inertial measurement sensor, the camera, etc.) in the bipedal robot, and after finishing the observation data, send the kinematic data at the previous moment, the kinematic data at the current moment, the inertial measurement data from the previous moment to the current moment, the previous moment image, the current moment image and the initial pose to the server, so that the server optimizes the initial pose to obtain the target pose; when the server executes the estimation process of the initial pose, the robot terminal can receive the observation data transmitted by each sensor (including a kinematic sensor, an inertial measurement sensor, a camera and the like) in the bipedal robot, and then can transmit the kinematic data at the last moment, the kinematic data at the current moment, the inertial measurement data from the last moment to the current moment, the previous moment image and the current moment image to the server so as to enable the server to estimate the initial pose, and optimize the estimated initial pose to obtain the target pose; the estimation process of the initial pose and the calculation process of the target pose can be independently executed by different servers respectively. Therefore, the data processing can be performed according to the processing capacity distribution of the robot terminal and the server, and the data processing speed can be improved.
In an embodiment, the initial pose of the camera at the current moment can be estimated by the kinematic data at the current moment and the inertial measurement data, specifically, the kinematic data at the current moment comprises the trunk motion data at the current moment of the bipedal robot, namely, the motion data of each component part from the waist to the head camera of the bipedal robot, so that the inertial measurement data from the last moment to the current moment can be integrated to obtain the initial pose data of the waist; then, according to a fourth motion chain from the waist of the bipedal robot to the camera, carrying out data conversion on trunk motion data at the current moment to obtain trunk initial conversion data; and then, according to the waist initial pose data and the trunk initial conversion data, calculating to obtain the initial pose of the camera at the current moment.
Because the inertial measurement unit is arranged at the waist position of the bipedal robot, inertial measurement data such as waist acceleration, angular velocity and the like from the last moment to the current moment can be measured by the inertial measurement unit, inertial measurement data from the last moment to the current moment can be integrated to obtain waist average acceleration and waist average angular velocity from the last moment to the current moment at the waist position, and waist initial pose data at the current moment can be estimated by using waist average acceleration, waist average angular velocity and waist historical pose data from the last moment.
The motion chain from the waist of the biped robot to the head camera can refer to a plurality of joint motion systems between the waist of the biped robot and the head camera, and the like, the conversion relation between the joint coordinate systems corresponding to each connected joint can be calculated through encoder data and connecting rod length under the joint coordinate system corresponding to the joint, so that the conversion relation between the waist coordinate system of the biped robot and the head camera coordinate system can be constructed according to the transmission sequence of the motion chain from the waist of the biped robot to the head camera and the conversion relation between the joint coordinate systems corresponding to each connected joint, and the trunk initial conversion data can be calculated, wherein the trunk initial conversion data can be represented as the conversion relation between the waist coordinate system of the biped robot and the head camera coordinate system. Because the waist initial pose data can be represented by matrix data, the trunk initial conversion data can be a conversion matrix, so matrix multiplication calculation can be carried out by utilizing the waist initial pose data and the trunk initial conversion data, and the initial pose of the camera at the current moment, namely the pose of the camera coordinate system under the waist coordinate system, is obtained. Because a great amount of noise information exists in the measured value of the sensor, especially peak noise generated by impact, vibration and the like when the bipedal robot lands, a great amount of noise information exists in the estimated pose, and the accuracy is low; in the related art, inertial measurement data acquired by an inertial measurement unit on a biped robot can be input into a preset prediction model to predict the motion state of a camera, so that the estimation of the pose of the camera is realized, but the accuracy of the implementation mode mainly depends on the accuracy of the prediction model, when the prediction model deviates more from the actual camera track, the estimation of the pose of the camera can drift along with accumulation of time, and the accuracy is naturally influenced. Therefore, the initial pose of the camera is obtained by fusing the waist initial pose obtained by integral prediction of the inertial measurement data and the trunk initial conversion data obtained by kinematic calculation of the kinematic data, so that not only can the drift phenomenon occurring when the pose is estimated by only using the inertial measurement data be avoided, but also the noise occurring when the pose is estimated by only using the kinematic calculation can be avoided, and the estimation precision of the initial pose of the camera at the current moment is improved.
Referring to fig. 5, fig. 5 is a schematic diagram illustrating an initial pose calculation manner according to an embodiment of the present application. It can be seen that the motion chain from the waist 410 to the head camera 510 of the bipedal robot includes the waist 410 joint, the head joint and the trunk portion between the waist 410 joint and the head joint, therefore, the conversion relationship between the camera coordinate system and the waist coordinate system can be constructed according to the waist kinematic data (such as the swing angle of the trunk portion relative to the Y axis in the waist 410 coordinate system shown in fig. 5) under the waist 410 coordinate system measured by the joint encoder of the waist 410 joint, the head kinematic data (such as the swing angle of the trunk portion relative to the Y axis in the camera coordinate system shown in fig. 5) under the head (i.e. the camera) coordinate system measured by the joint encoder of the head joint, and the length of the trunk portion, and the initial conversion data of the trunk can be calculated. The waist 410 joint may be provided with an inertial measurement unit in addition to the joint encoder, and the inertial measurement unit may measure a moving speed acceleration and an angular velocity of the bipedal robot centering on the waist 410 of the bipedal robot. Therefore, the inertial measurement data from the last time to the current time measured by the inertial measurement unit can estimate the waist initial pose data at the current time, and then the waist initial pose data is multiplied by the trunk initial conversion data to obtain the initial pose of the camera at the current time, namely the initial pose of the camera coordinate system under the waist coordinate system.
In an embodiment, the initial pose of the camera at the current moment may be obtained by estimating the kinematic data at the current moment, specifically, the kinematic data at the current moment includes lower limb motion data at the current moment and trunk motion data at the current moment, where the lower limb motion data may include kinematic data between a waist and a foot, and the trunk motion data may include kinematic data between a waist and a head, so that pose prediction may be performed on the lower limb motion data at the current moment and the trunk motion data at the current moment to obtain the initial pose of the camera at the current moment. The motion state of the head camera position in the biped robot is obtained, and then initial pose estimation of the camera at the current moment is realized.
Referring to fig. 6, fig. 6 is a schematic diagram illustrating motion state prediction according to an embodiment of the present application. As shown in fig. 6, by acquiring encoder data of each joint of the bipedal robot, that is, trunk motion data and lower limb motion data, and then inputting the trunk motion data and the lower limb motion data into a kinematic prediction model of the bipedal robot to predict a motion state, the kinematic prediction model is generated by training with historical data (including historical trunk motion data, historical lower limb motion data, historical motion state, historical pose, and the like), so that the motion state of the bipedal robot corresponding to the currently input trunk motion data and lower limb motion data can be obtained, and the motion state of the head camera can be determined, and further, the initial pose of the camera at the current moment can be estimated. As shown in fig. 6, the motion state predicted by using the trunk motion data and the lower limb motion data at the current moment is a state in which the left foot of the biped robot is lifted and the right foot is supported, so that the motion state of the head camera can be determined according to the swing track of the biped gait of the biped robot, and the initial pose can be calculated.
Step 340: and calculating to obtain the kinematic residual error of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose.
In an embodiment, the kinematic residual may refer to a pose deviation between a pose calculated by a camera using an actual observed value and an estimated fitted pose, that is, a difference between a camera motion calculated by kinematic data at a previous time and kinematic data at a current time and a camera motion calculated by using an initial pose estimated fit, specifically, an actual motion of the camera may be calculated by kinematic data at a previous time and kinematic data at a current time, and a predicted motion of the camera may be calculated by using a historical pose at a previous time and an initial pose at a current time, so as to calculate a deviation between the actual motion and the predicted motion of the camera, that is, the pose deviation of the camera may be determined by using the kinematic data of the camera. Therefore, errors of the actual pose and the predicted pose of the camera can be found through the kinematic residual error, and accordingly the initial pose can be optimized according to the kinematic residual error, and the kinematic residual error is reduced to improve accuracy of camera pose estimation. In addition, abnormal conditions in the process of estimating the camera motion can be identified by calculating the kinematic residual error, and when the kinematic residual error exceeds a preset residual error threshold, the camera positioning can be considered to be problematic, such as error of kinematic data or initial pose, so that data rollback or reinitialization operation can be performed in time, and the robustness of the VSLAM system is improved. Meanwhile, the VSLAM system can be visually debugged by the kinematic residual, and the quality and the accuracy of the camera pose estimation can be known by observing the change condition of the kinematic residual, so that the abnormal condition in the VSLAM system can be quickly identified.
In an embodiment, in the process of calculating the kinematic residual error of the camera according to the kinematic data at the previous time, the kinematic data at the current time and the initial pose, the kinematic incremental data of the camera can be calculated according to the kinematic data at the previous time and the kinematic data at the current time; and then calculating to obtain the kinematic residual error of the camera according to the kinematic increment data and the initial pose. The motion variation of the camera from the last moment to the current moment is estimated by comparing the numerical deviation between the kinematic data at the last moment and the kinematic data at the current moment, namely motion increment data, and further performing kinematic calculation by utilizing the motion increment data, then the motion gesture of the camera at the current moment is determined, and then the motion gesture of the camera is compared with the initial gesture, so that the kinematic residual error of the camera is calculated. Or, the motion gesture of the camera at the previous moment is obtained by performing kinematic calculation on the kinematic data at the previous moment, and the motion gesture of the camera at the current moment can be obtained by performing kinematic calculation on the kinematic data at the current moment, and then the difference value calculation is performed on the motion gesture of the camera at the previous moment and the motion gesture at the current moment, so that the motion increment data of the camera at the previous moment to the current moment, namely the variation quantity of the motion gesture of the camera at the previous moment to the current moment, can be obtained. The accuracy of the pose calculated by the actual observation value can be improved by a calculation mode of calculating the motion increment data preferentially and then utilizing the motion increment data and the initial pose to perform the motion residual error, and the accuracy of the motion residual error can be improved.
Referring to fig. 7, fig. 7 is a schematic diagram of motion delta data provided by an embodiment of the present application. Calculating the kinematic data at the previous moment to obtain the motion gesture of the bipedal robot at the previous moment and the motion gesture of the camera at the previous moment, calculating the kinematic data at the current moment to obtain the motion gesture of the bipedal robot at the current moment and the motion gesture of the camera at the current moment, wherein the dotted line part shown in fig. 7 represents the motion gesture of the bipedal robot at the previous moment, the solid line part represents the motion gesture of the bipedal robot at the current moment, meanwhile, the coordinate system { c } of the head position of the bipedal robot is represented as a camera coordinate system, the coordinate system { b } of the waist position of the bipedal robot is represented as a waist coordinate system, the coordinate system { s } of the foot position of the bipedal robot is represented as a support foot coordinate system, the change amount of the motion gesture of the bipedal robot at the previous moment to the current moment can be seen by comparing the motion gesture of the bipedal robot at the previous moment and the current moment, and accordingly, the motion increment data of the camera at the previous moment to the current moment can be obtained by comparing the motion gesture of the camera at the previous moment to the current moment, and the motion increment data can be obtained by comparing the motion data of the motion gesture at the moment at the previous moment to the current moment The representation, wherein (fk) can be expressed as a kinematic solution, i can represent the last time, i+1 can be the current time, thus +.>I.e. the motion delta data may be expressed as delta motion of the camera in the camera coordinate system from the last moment to the current moment by means of a kinematic data solution.
In an embodiment, in the process of calculating the kinematic residual error of the camera according to the motion increment data and the initial pose, the historical pose of the camera at the last moment can be acquired first; then, calculating according to the historical pose and the initial pose to obtain the pose variation of the camera; and then calculating to obtain the kinematic residual error of the camera according to the kinematic increment data and the pose variation. The historical pose of the camera at the last moment can be stored in the robot terminal or the server; the historical pose of the camera at the previous moment can be the initial pose corresponding to the previous moment of the camera, the initial pose corresponding to the previous moment of the key image, the target pose corresponding to the previous moment of the camera, and the target pose corresponding to the previous moment of the key image. By comparing the historical pose of the camera at the last moment with the initial pose at the current moment, the pose change amount of the estimated and fitted pose from the last moment to the current moment can be calculated, and the predicted and estimated camera incremental motion can be determined. Therefore, by comparing the deviation between the camera incremental motion calculated by the kinematic data and the camera incremental motion estimated by prediction, that is, calculating the kinematic residual error by using the motion incremental data and the pose variation amount, the difference between the camera motion calculated by the kinematic data (that is, the camera motion actually observed) and the camera motion predicted by the motion model and the initial pose can be more accurately determined, and the initial pose can be optimized to obtain a more accurate target pose.
Referring to fig. 8, fig. 8 is a schematic diagram illustrating calculation of kinematic residuals according to an embodiment of the present application. As shown in the left-hand sub-graph of fig. 8, the motion delta data of the camera can be calculated from the previous time kinematic data and the current time kinematic data. As shown in the right sub-graph in fig. 8, by comparing the historical pose of the camera at the last time and the initial pose at the current time, the pose change amount of the estimated and fitted pose from the last time to the current time can be calculated. Then, as shown in the middle subgraph in fig. 8, by comparing the deviation between the motion delta data calculated from the kinematic data and the pose change amount estimated by prediction, the kinematic residual can be calculated.
Step 350: and calculating the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image.
In an embodiment, the motion state of the bipedal robot with the waist as the center can be determined by combining the inertial measurement data from the previous moment to the current moment, the motion of the feature points corresponding to the images can be captured by combining the previous moment image and the current moment image, further, the visual motion incremental data of the camera can be obtained by analyzing the information such as the feature point matching and the motion track corresponding to the images, and then the accuracy and the robustness of pose estimation can be improved by combining the kinematic residual error, the inertial measurement data and various observation data of the images shot by the camera. The fast motion of the camera in a short time can be deduced by utilizing the kinematic data measured by the kinematic sensor and the inertial measurement data measured by the inertial measurement unit, so that the pose update of the camera can be realized by the inertial measurement data or the kinematic residual error, and the kinematic residual error and the inertial measurement data are fused as the kinematic residual error reflects the deviation between the actually observed pose and the estimated pose, so that the error of pose estimation can be corrected, and the precision of camera pose estimation is improved. However, the pose estimation is completed by utilizing the kinematic residual error and the inertial measurement data and depends on the relation between adjacent moments, and the algorithm which depends on local constraint and is continuously performed in a chain manner can cause the gradual accumulation of errors and generate larger error drift; the motion of the camera can be deduced by using the data of the vision sensor, namely, the image shot by the camera, and error drift is not easy to generate, so that the motion residual error, the inertia measurement data and the shot image are fused, the pose estimation of the rapid motion in a short time can be realized, the error generated by the motion residual error and the inertia measurement data can be reduced, the effect of improving the precision and the stability of the pose of the camera can be further achieved, and a more accurate target pose can be obtained.
In an embodiment, in the process of calculating the target pose of the camera at the current moment, pre-integration processing can be performed on the inertial measurement data to obtain an inertial data residual error; then, carrying out feature extraction and feature matching processing on the previous moment image and the current moment image to obtain a visual re-projection error; and then calculating to obtain the target pose of the camera at the current moment according to the kinematic residual error, the inertial data residual error and the visual reprojection error. Specifically, a segment integration method can be adopted to integrate the inertial measurement data from the last time to the current time and construct a least square residual error as an inertial data residual error, and through pre-integration processing, the accuracy and stability of camera attitude estimation can be improved, the integration error of an inertial measurement unit can be corrected, and the accumulated error caused by inaccurate attitude estimation can be reduced. And integrating all inertial measurement data from the previous moment to the current moment and constructing a least square residual as an inertial data residual.
For the calculation of the visual re-projection error, feature extraction can be performed on the previous moment image and the current moment image, a corner-based rapid feature detection algorithm ((Features from Accelerated Segment Test, FAST) and a feature point description algorithm (Binary Robust Independent Elementary Features, BRIEF) can be used for detecting key points in the image by using a directional rapid feature detection and feature point description algorithm (Oriented Fast and Rotated Brief, ORB) fused, descriptors of the key points are calculated and used for performing subsequent feature matching, then, according to the previous moment key point descriptor in the previous moment image and the current moment key point descriptor in the current moment image, a feature matching algorithm (such as a nearest neighbor matching algorithm, an optimal matching algorithm and the like) is adopted to determine the corresponding relation between the previous moment key point descriptor and the current moment key point descriptor, so that matched feature points are found, then, the visual re-projection error is obtained by calculating pixel coordinate differences between the feature points corresponding to match, and the visual re-projection error can be directly observed to obtain the posture change about the image, so that the visual re-projection error can be used for adjusting and correcting the camera pose, and the camera pose accuracy is improved.
After the kinematic residual error, the inertial data residual error and the visual re-projection error are obtained, a least square problem can be constructed, and an algorithm for solving a function extremum by iteration is utilized to solve, so that the target pose of the camera at the current moment is obtained. By using the kinematic residual, the inertial data residual and the visual re-projection error, the target pose of the camera at the current moment can be calculated. The residual amounts represent error information of different sources, and the tight coupling of the multi-source observation data is realized by fusing the error information of a plurality of observation data, so that the accuracy and stability of pose estimation can be improved, and a more accurate target pose can be obtained.
Wherein, a residual function can be constructed by using the kinematic residual, the inertial data residual and the visual re-projection error, and the specific residual function is as follows:
wherein r is kin Representing kinematic residual error, r imu Representing inertial data residual, r cam Representing the visual re-projection error, x i The state quantity which is expressed as the state quantity which needs to be optimized in the pose of the camera specifically comprises the following state quantity:
wherein,for the position of the camera,>for the pose of the camera, ">For the movement speed measured by the inertial measurement unit, -a movement speed of the inertial measurement unit>Bias data for the measurement of the accelerometer in the inertial measurement unit, >Offset data for the measurement of gyroscopes in inertial measurement units.
It should be noted that in practical applications, the specific methods of pre-integration processing and image feature extraction and feature matching processing may vary depending on the specific algorithms, sensors and application scenarios. Therefore, in the actual application scene, the adjustment and optimization can be performed according to specific situations.
Referring to fig. 9, fig. 9 shows a flow diagram of a data processing method in the related art. In the related art, based on a robust depth image dense SLAM framework, a robot motion state estimation module (State Estimation Robot Walking, ser) is added, and the robot motion state estimation module can perform state estimation through data measured by an inertial measurement unit, data measured by a joint encoder, and data measured by a stress sensor, and a specific flow is shown in fig. 9. Firstly, estimating the pose of a robot by utilizing a visual odometer, inputting the obtained pose into a robot motion state estimation module to correct and optimize the estimated pose, and inputting the optimized pose into a local mapping module to perform map construction, wherein the pose is processed by utilizing data measured by an inertial measurement unit, data measured by a joint encoder and data measured by a stress sensor in the robot motion state estimation module in a post-processing mode, and errors are accumulated in all the data, so that the accuracy of the obtained pose is poor. The method provided by the embodiment of the application can acquire the kinematic data of the biped robot at the last moment, the kinematic data of the current moment and the inertial measurement data from the last moment to the current moment, can acquire the last moment image and the current moment image shot by the camera arranged at the head position of the biped robot, and can acquire the initial pose of the camera at the current moment, so that the kinematic residual error of the camera can be calculated according to the kinematic data of the last moment, the kinematic data of the current moment and the initial pose, and the kinematic residual error of the camera can be calculated, therefore, the motion change of the biped robot at the adjacent moment can be calculated, the deviation between the data calculated by the kinematic data (namely the actual observed transformation amount) and the data calculated by the initial pose (namely the estimated transformation amount) can be determined, thereby being beneficial to correcting the camera pose and reducing the influence of the motion blur of the image. Then, according to the kinematic residual error, the inertial measurement data from the last moment to the current moment, the last moment image and the current moment image, the target pose of the camera at the current moment is obtained through calculation, and the initial pose is optimized and adjusted by combining the motion relation between the inertial measurement data and the kinematic data at a plurality of moments and the visual projection relation between the last moment image and the current moment image, so that the tight coupling of multi-source information can be realized, the robustness of pose estimation can be improved, and more accurate pose data of the bipedal robot can be obtained.
In an embodiment, since the motion delta data and the pose change amount can be used to describe the pose change of the camera from the last moment to the current moment, and the pose change includes the position change of the camera and the pose change of the camera, displacement delta data for describing the position change of the camera obtained by the calculation of the kinematic data and rotation delta data for describing the pose change of the camera obtained by the calculation of the kinematic data are included in the motion delta data; accordingly, displacement change data for describing a change in the position of the camera from the initial pose estimation and rotation change data for describing a change in the pose of the camera from the initial pose estimation are included in the pose change amount. Therefore, in the process of calculating the kinematic residual error of the camera, the displacement increment data and the displacement change data can be subjected to difference value calculation to obtain a displacement change value, and logarithmic mapping is performed according to the rotation increment data and the rotation change data to obtain an attitude change value; and then, constructing and obtaining a kinematic residual error of the camera according to the displacement change value and the posture change value.
Referring to fig. 10, fig. 10 is a schematic diagram illustrating pose change according to an embodiment of the present application. The pose change of the camera from the previous time to the current time includes the position change of the camera 510 and the pose change of the camera 510, as shown in fig. 10, the dashed line portion represents the position and the pose of the camera 510 and the waist 410 from the previous time, and the solid line portion represents the position and the pose of the camera 510 from the current time, and the position change of the camera 510 from the previous time to the current time can be obtained by comparing the position of the camera 510 from the previous time to the position of the camera 510 at the current time based on the waist coordinate system. Accordingly, the pose of the camera 510 at the previous time and the pose of the camera 510 at the current time can be compared with each other by taking the waist coordinate system as a reference, so as to obtain the change of the pose of the camera 510 from the previous time to the current time. The position change of the camera 510 and the posture change of the camera 510 can be determined by taking the waist coordinate system as a reference, in other words, the position change and the posture change of the camera 510 can be calculated by calculating the position change and the posture change of the camera 510, thereby simplifying the data processing amount and improving the data processing efficiency.
The displacement increment data can be a position change vector of the camera from the last moment to the current moment through the solution of the kinematic data, the displacement change quantity can be a position change vector of the camera from the last moment to the current moment through the calculation of the initial pose and the historical pose, and therefore the deviation between the actually observed displacement quantity and the displacement quantity obtained through estimation fitting, namely a displacement change value, can be obtained through the difference calculation of the displacement increment data and the displacement change data. Correspondingly, the rotation increment data can be a gesture rotation matrix for gesture conversion from the last moment to the current moment of the camera through a kinematic data solution, and the rotation change data can be a gesture rotation matrix for gesture conversion from the last moment to the current moment of the camera through an initial gesture and a historical gesture calculation, so that the number of the plums can be converted into a number of lie algebra according to logarithmic mapping of the rotation increment data and the rotation change data, gesture change values are obtained, and then the gesture change values and displacement change values are utilized for matrix construction to obtain kinematic residual errors, thereby realizing conversion of nonlinear operation into linear operation, simplifying the solving and gesture optimizing process of the least square problem, and simultaneously having high calculation efficiency and good numerical stability of the number of the lie algebra.
The specific formula of the kinematic residual is as follows:
wherein,displacement increment data representing the last time to the current time,/->Indicating the displacement variation from the last moment to the current moment,/-, for example>Rotation increment data indicating the last time to the current time,/->The LogSO3 function is a logarithmic mapping function representing the amount of rotation change from the last time to the current time.
In an embodiment, the kinematic data at the previous moment includes the foot stress data at the previous moment, the lower limb motion data at the previous moment and the trunk motion data at the previous moment, and the kinematic data at the current moment includes the foot stress data at the current moment, the lower limb motion data at the current moment and the trunk motion data at the current moment; in the process of calculating the motion increment data of the camera, the supporting foot at the previous moment can be determined according to the foot stress data at the previous moment, and the supporting foot at the current moment can be determined according to the foot stress data at the current moment; then, according to a first motion chain from the last moment supporting feet to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the last moment to obtain lower limb conversion data at the last moment, and according to a second motion chain from the current moment supporting feet to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the current moment to obtain lower limb conversion data at the current moment; according to a third motion chain from the waist of the bipedal robot to the camera, respectively carrying out data conversion on trunk motion data at the previous moment and trunk motion data at the current moment to obtain trunk conversion data at the previous moment and trunk conversion data at the current moment; and then calculating to obtain the motion increment data of the camera according to the lower limb conversion data at the last moment, the lower limb conversion data at the current moment, the trunk conversion data at the last moment and the trunk conversion data at the current moment.
By judging the supporting feet of the biped robot, the lower limb conversion data at the corresponding moment is solved by utilizing the motion chain from the supporting feet to the waist at the corresponding moment, so that the conversion relation between the waist coordinate system and the supporting foot coordinate system can be constructed, different conversion data can be calculated according to different supporting feet, and the data error is reduced. The pose of the camera is affected by the motion of lower limbs and the motion of the trunk, and trunk conversion data at corresponding moments are solved by utilizing a motion chain between the waist and head cameras, so that the conversion relation between a waist coordinate system and a camera coordinate system can be constructed. After the trunk conversion data and the lower limb conversion data are obtained, the lower limb movement and the trunk movement can be jointly considered in the support foot coordinate system to obtain the pose description of the camera coordinate system under the support foot coordinate, so that the movement condition of the camera can be more accurately described. Therefore, the camera pose description of the camera coordinate system under the support foot coordinate at the previous moment can be obtained through the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment, and the camera pose description of the camera coordinate system under the support foot coordinate at the current moment can be obtained through the lower limb conversion data at the current moment and the trunk conversion data at the current moment, so that the motion increment data of the camera can be calculated according to the camera pose description at the two moments. The bipedal gait of the bipedal robot can cause the foot to generate a swing track, meanwhile, the impact force generated by the foot striking the ground can cause image blurring, and the supporting foot is the part of the bipedal robot which is in direct contact with the ground in the gait movement, and the movement of the supporting foot is closely related to the overall balance and stability of the bipedal robot. Therefore, the motion characteristics of the bipedal robot can be more accurately captured by selecting the motion chain corresponding to the support foot, and the reliability and the precision of the motion increment data are improved.
In an embodiment, referring to fig. 11, fig. 11 is a schematic diagram of a bipedal robot in different supporting states according to an embodiment of the application. The foot stress data can be measured by pressure sensors arranged on the feet of the bipedal robot, the lower limb movement data can be measured by joint encoders arranged between the waist and the feet of the bipedal robot, and the trunk movement data can be measured by joint encoders arranged between the waist and the head cameras of the bipedal robot. The supporting state of the biped robot can be judged through the foot stress data of the biped robot, the supporting state of the biped robot comprises a left foot single-foot supporting state, a right foot single-foot supporting state and a left foot and right foot simultaneous supporting state, when the foot stress data of a left foot 420 of the biped robot exceeds a preset stress value, and the foot stress data of a right foot does not exceed the preset stress value, the biped robot is in the left foot single-foot supporting state, the corresponding supporting foot is the left foot 420, and as shown in a left side sub-graph in fig. 11, a solid line part in the biped robot indicates that the left foot 420 is the supporting foot; when the right foot stress data of the biped robot exceeds the preset stress value and the left foot stress data does not exceed the preset stress value, the biped robot is in a right foot single-foot supporting state, the corresponding supporting foot is a right foot 430, as shown in a middle sub-graph in fig. 11, a solid line part in the biped robot indicates that the right foot 430 is the supporting foot; when the foot force data of the left foot 420 and the right foot 430 of the biped robot exceed the preset force values, it is indicated that the biped robot is in a state of supporting both the left foot and the right foot, and the corresponding supporting feet are the left foot 420 and the right foot 430, as shown in the right side sub-graph in fig. 11, the solid line parts in the biped robot indicate that the left foot 420 and the right foot 430 are both supporting feet.
Accordingly, when the support foot is determined to be the left foot, the first kinematic chain or the second kinematic chain may be a plurality of joints or the like between the left foot and the waist of the bipedal robot; when the supporting foot is determined to be the right foot, the first moving chain or the second moving chain can be a plurality of joints and other moving systems between the right foot and the waist of the bipedal robot; when the support foot is determined to be a left and right biped, the first kinematic chain or the second kinematic chain may be a motion system such as a plurality of joints between the left and right biped of the biped robot to the waist, or may be a motion system such as a plurality of joints between the right biped robot to the waist. The third kinematic chain refers to a kinematic system of the bipedal robot such as a plurality of joints between the waist to head cameras. In gait movement, the support feet are switched to cause jump of the whole movement of the biped robot, if the fixed coordinate systems such as the coordinate system of the same support foot are kept, errors can be easily caused in data conversion to influence the accuracy of movement increment data, so that the continuity of data can be kept between the support feet in a switching mode by adopting corresponding movement chains according to the support feet at different moments, the influence of the discontinuity of the kinematic data on the movement estimation of a camera is reduced, and the accuracy of the movement increment data is improved.
Referring to fig. 12, fig. 12 is a schematic diagram illustrating a kinematic data calculation of a bipedal robot according to an embodiment of the application. The kinematic data comprise coding data (namely trunk motion data) of a joint encoder between the waist and the camera, coding data (namely lower limb motion data) of a lower limb joint encoder and foot stress data, and the supporting foot can be judged by analyzing the foot stress data, so that the lower limb motion data is subjected to kinematic calculation according to the supporting foot, and lower limb conversion data is obtained. The motion data of the trunk can be directly subjected to kinematic calculation to obtain trunk conversion data.
In an embodiment, in the process of calculating the motion increment data of the camera, the foot pose transformation data can be determined according to the support foot data at the previous moment and the support foot data at the current moment, and matrix transposition is performed according to the lower limb transformation data at the previous moment and the trunk transformation data at the previous moment to obtain pose transposition data at the previous moment; and multiplying the bipedal pose transformation data, the pose transposition data at the previous moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain the motion increment data of the camera.
In an embodiment, the foot pose transformation data may be used to represent a motion change of the support foot at a previous moment and a current moment, the pose transformation data at the previous moment may be used to represent a camera pose of the support foot coordinate system corresponding to the support foot at the previous moment in the camera coordinate system at the previous moment, and the lower limb transformation data at the current moment and the trunk transformation data at the current moment may be multiplied to obtain data for representing a camera pose of the camera coordinate system at the current moment in the support foot coordinate system corresponding to the support foot at the current moment. Therefore, by multiplying the biped pose transformation data, the pose transposition data at the previous moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain the motion increment data of the camera, the influence of the biped robot on the camera pose caused by the support foot transformation and the trunk motion during the biped gait motion can be comprehensively considered, the pose change of the camera can be more comprehensively described, and the pose estimation accuracy is improved.
Referring to fig. 13, fig. 13 is a schematic diagram showing a conversion relationship of a motion coordinate system of a bipedal robot according to an embodiment of the application. The camera pose in the camera coordinate system can be converted into the camera pose in the waist coordinate system by utilizing the trunk conversion data, and the camera pose in the waist coordinate system can be converted into the camera pose in the support foot coordinate system by utilizing the lower limb conversion data. When the support foot at the previous moment is the same as the support foot at the current moment, the coordinate system of the support foot is fixed, so that the movement change of the camera can be calculated by using the camera pose at the previous moment and the camera pose at the current moment; when the support foot at the previous moment is different from the support foot at the current moment, the camera pose of the camera coordinate system under the non-support foot coordinate system can be converted into the camera pose of the camera coordinate system under the same support foot coordinate system by utilizing bipedal pose conversion data, so that the movement change of the camera can be accurately calculated.
In an embodiment, matrix multiplication can be performed on the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment to obtain pose conversion data at the previous moment, and then matrix transposition is performed on the pose conversion data at the previous moment to obtain pose transposition data at the previous moment, wherein a specific calculation formula of motion increment data of the camera is as follows:
where (fk) may be expressed as a kinematic solution, i may represent the last time, i+1 may be expressed as the current time, and thus,i.e. the motion delta data may be expressed as delta motion of the camera in the camera coordinate system from the last moment to the current moment by means of a kinematic data solution. />The waist pose of the waist coordinate system under the support foot coordinate system at the last moment can be represented; />The camera pose of the camera coordinate system under the waist coordinate system at the last moment can be represented; />The bipedal pose transformation data can be represented; />The waist pose of the waist coordinate system under the support foot coordinate system at the current moment can be represented; />The camera pose of the camera coordinate system under the waist coordinate system at the current moment can be represented.
Therefore, the consistency of the data in the calculation process can be maintained by matrix transposition of the pose conversion data at the previous moment, the bipedal pose transposition data at the previous moment, the lower limb conversion data at the current moment and the trunk conversion data at the current moment are conveniently multiplied, the movement change of the camera at the previous moment and the current moment can be judged, the movement increment data of the camera is obtained, and the calculation system efficiency and accuracy of the movement increment data are improved.
In an embodiment, the foot pose transformation data may be used to represent the motion change of the supporting foot at the previous moment and the supporting foot at the current moment, so when the supporting foot at the previous moment is the same as the supporting foot at the current moment, it is indicated that the supporting state of the biped robot is not changed, that is, the foot of the non-supporting foot of the biped robot does not strike the ground from the previous moment to the current moment, and the swing track of the biped gait has no influence on the pose change of the camera, and the identity matrix may be used as the foot pose transformation data; when the supporting foot at the previous moment is different from the supporting foot at the current moment, the supporting state of the biped robot is changed, namely, the feet of the biped robot collide with the ground from the previous moment to the current moment, and the swing track of the biped gait has an influence on the pose change of the camera.
When the supporting foot at the previous moment is a left foot and a right foot and the supporting foot at the current moment is a right foot, the supporting state of the bipedal robot is changed, and according to the motion chain between the bipedal robot at the previous moment and the bipedal robot at the current moment, the lower limb motion data in the state of simultaneously supporting the left foot and the right foot can be utilized for calculation to obtain foot position posture conversion data, or according to the motion chain between the bipedal robot at the previous moment and the bipedal robot at the current moment, the lower limb motion data in the state of simultaneously supporting the left foot and the right foot and the lower limb motion data at the current moment can be calculated to obtain foot position posture conversion data; when the supporting foot at the previous moment is a left foot and the supporting foot at the current moment is a left foot and a right foot, the lower limb movement data in the state of simultaneously supporting the left foot and the right foot can be used for calculation according to the movement chain between the two feet of the bipedal robot at the previous moment and the current moment to obtain foot position posture transformation data, or the lower limb movement data in the state of simultaneously supporting the left foot and the right foot and the lower limb movement data at the previous moment can be calculated according to the movement chain between the two feet of the bipedal robot at the previous moment and the current moment to obtain foot position posture transformation data; when the last moment supporting foot is the left foot and the current moment supporting foot is the right foot, the lower limb movement data in the state of simultaneously supporting the left foot and the right foot can be utilized to calculate according to the movement chain between the two feet of the bipedal robot at the last moment and the current moment to obtain foot position and pose transformation data, or the lower limb movement data in the state of simultaneously supporting the left foot and the right foot, the lower limb movement data at the last moment and the lower limb movement data at the current moment can be calculated according to the movement chain between the two feet of the bipedal robot at the last moment and the current moment to obtain foot position and pose transformation data.
In an embodiment, after calculating the target pose of the camera at the current moment, the data processing method for the bipedal robot further includes:
if the current moment image is confirmed to be a key image, calculating to obtain a kinematic secondary residual error of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the target pose;
acquiring a historical key image and historical inertial measurement data of corresponding moments of the historical key image;
and according to the kinematic secondary residual error, the inertial measurement data, the historical key image and the current moment image, carrying out optimization processing on the target pose and map points corresponding to the current moment image to obtain the optimized key pose and key map points.
In an embodiment, after the target pose of the camera at the current moment is calculated, the current moment image may be detected, and whether the current moment image may be used as a key image may be determined. The key image is one representative image in a plurality of images corresponding to a certain period of time, because the frequency of shooting images by the camera is high, the information redundancy between two images adjacent in time is high, meanwhile, the data volume of the images shot by the camera is huge, if all the images are extracted to extract characteristic points, the data processing time is long, and the key image is detected to be one representative image in a plurality of images in local time, so that the information redundancy can be reduced, and the data processing time is shortened. The key image is mainly used for optimizing errors in the local map thread and the closed loop detection thread, and is used for marking map points in the local map thread and generating a local map, and the image quality of the key image has great influence on tasks such as error optimization, map construction and the like, so that whether the image meets preset detection conditions or not needs to be judged, and the preset detection conditions can comprise one or more of the following conditions: the number of the determined key images is smaller than the number of the preset images, the time interval from the last key image is larger than the preset time interval, the number of the three-dimensional points obtained after feature matching of the current moment image is larger than a preset three-dimensional point threshold value, and the like. When the current time image meets the preset detection condition, the current time image can be confirmed to be a key image. Meanwhile, the map points in the key images can be determined by carrying out feature extraction and feature matching on the key images.
In an embodiment, the kinematic secondary residual error may refer to a pose deviation between a pose calculated by a camera using an actual observed value and a target pose, that is, a difference between a camera motion calculated by kinematic data at a previous time and kinematic data at a current time and a camera motion calculated by using the target pose, specifically, an actual motion of the camera may be calculated by kinematic data at the previous time and kinematic data at the current time, and a predicted motion of the camera may be calculated by calculating a target historical pose at the previous time and a target pose at the current time, so as to calculate a deviation between the actual motion and the predicted motion of the camera, that is, the pose deviation of the camera may be determined by secondarily using the kinematic data of the camera and the target pose. Therefore, the error between the actual pose of the camera and the adjusted target pose can be found through the kinematic secondary residual error, so that the target pose can be secondarily optimized according to the kinematic secondary residual error, and the kinematic secondary residual error is reduced to improve the accuracy of camera pose estimation.
In an embodiment, the historical key image and the historical inertial measurement data of the moment corresponding to the historical key image may be stored in the robot terminal or may be stored in the server; the historical key images and the historical inertia measurement data are in one-to-one correspondence, only the adjacent historical key images, namely the last key image, are obtained, the historical inertia measurement data at the moment corresponding to the last key image are correspondingly obtained, likewise, the key motion state of the bipedal robot with the waist as the center can be determined by combining the historical inertia measurement data and the inertia measurement data, the motion of the characteristic points corresponding to the key images in vision can be captured by combining the historical images and the images at the current moment, further, the key vision motion increment data of the camera can be obtained by analyzing the information such as the characteristic point matching and the motion track corresponding to the key images, and then the target pose is adjusted again by fusing the kinematic secondary residual error, the historical inertia measurement data and various observation data of the key images, so that the accuracy and the robustness of pose estimation can be improved.
In an embodiment, in the process of optimizing the map points corresponding to the target pose and the current moment image according to the kinematic secondary residual error, the inertial measurement data, the historical key image and the current moment image, pre-integration processing can be performed according to the historical inertial measurement data and the inertial measurement data to obtain the key inertial data residual error; then, carrying out feature extraction and feature matching treatment on the historical key image and the current moment image to obtain a key vision reprojection error; and then, optimizing the target pose and map points corresponding to the current moment image according to the kinematic secondary residual error, the key inertial data residual error and the key visual re-projection error.
In an embodiment, a plurality of historical key images can be obtained simultaneously, and then all the historical inertial measurement data corresponding to each historical key image are obtained accordingly, and key inertial data residual errors are obtained by performing pre-integration processing on the inertial measurement data and the plurality of historical inertial measurement data.
In an embodiment, in the process of optimizing the target pose and the map point corresponding to the image at the current moment, the historical key pose of the image at the corresponding moment of the historical key image may be obtained, and feature extraction and feature matching are performed on the historical key image to obtain the historical key map point, so that a priori residual item is constructed by using the historical key pose, the historical key map point, the historical inertia measurement data and the like, the priori residual item may refer to a difference between priori information and a priori estimated value, the priori information may refer to previous knowledge or pre-existing information provided in the current local map thread, and the priori estimated value may be obtained by estimating the historical key pose, the historical key map point and the historical inertia measurement data, for example, by machine learning, constructing a least square method and a maximum likelihood estimation algorithm. The prior residual is obtained by subtracting the prior information from the prior estimated value, then the jacobian matrix of the prior residual is multiplied by the to-be-optimized state matrix constructed by the target pose corresponding to each key image and the map points to obtain a to-be-processed residual matrix, and then the prior residual is subtracted from the to-be-processed residual matrix to obtain a prior residual item. After obtaining the prior residual terms, the kinematic secondary residual, the key inertial data residual and the key visual re-projection error, a secondary residual formula can be obtained, and a least square problem is constructed, wherein the specific secondary residual formula is as follows:
Wherein r is kin Representing kinematic secondary residual error, r imu Representing inertial data residual, r cam Representing the visual re-projection error, χ is represented as a target pose and a corresponding map point, and specifically includes the following state quantities:
χ＝[x n ，x n+1 ，…，x n+N ，λ m ，λ m+1 ，…，λ m+M ]
wherein x is n Represents the nth target pose and is the starting pose in the sliding window, N represents the number of target poses in the sliding window,for the position of the camera,>for the pose of the camera, ">For the movement speed measured by the inertial measurement unit, -a movement speed of the inertial measurement unit>Bias data for the measurement of the accelerometer in the inertial measurement unit,>offset data for the measurement of gyroscopes in inertial measurement units. And lambda is m Represents the mth map point and is the starting map point observed by all the key images in the sliding window, and M is the number of map points observed by all the key images in the sliding window. r is (r) p As a priori residual error, J p And the jacobian matrix corresponding to the prior residual error is obtained.
The method comprises the steps of tightly coupling inertial measurement data, kinematic data and key images by adopting an edge mode based on a sliding window, calculating key inertial data residual errors and key vision re-projection residual errors by utilizing historical inertial measurement data, historical key images and images at the current moment, and then combining kinematic secondary residual errors, key inertial data residual errors and key vision re-projection residual errors into the sliding window to optimize map points corresponding to the target pose and the images at the current moment. With the increase of time, the data volume of the inertial measurement data, the kinematic data and the key images is gradually increased, so that the calculation complexity is higher and higher, and therefore, the processing can be performed in an edge mode, when the historical key pose and the historical key map point corresponding to the key images are removed, the constraint relation related to the adjacent key images is added into the optimization problem of the pose and the map point by using the sliding window as a priori term, so that important information can be prevented from being lost, and when the historical data is required to be discarded, the priori residual term can be used for replacing the chemical data required to be discarded, so that the transmission of the key information corresponding to the historical key images is completed.
After the least square problem is constructed, an algorithm for solving the function extremum through iteration can be utilized to solve, and the target pose of the camera at the current moment is obtained. And optimizing the target pose and the map points corresponding to the current moment image by using the prior residual error item, the kinematic residual error, the inertial data residual error and the visual re-projection error, so that the optimized key pose and the key map points can be obtained. Because these residual amounts represent error information of different sources, the accuracy and stability of pose estimation and map point estimation can be improved by fusing the error information of multi-source information, more accurate key poses and key map points can be obtained, and the accuracy of map construction can be further improved.
It should be noted that in practical applications, the specific methods of pre-integration processing and image feature extraction and feature matching processing may vary depending on the specific algorithms, sensors and application scenarios. Therefore, in the actual application scene, the adjustment and optimization can be performed according to specific situations.
In an embodiment, in the process of calculating the kinematic secondary residual error of the camera, the historical key pose of the moment corresponding to the historical key image can be obtained first, and the key pose conversion amount of the camera can be calculated according to the historical key pose and the target pose; then, calculating to obtain key motion increment data of the camera according to the kinematic data at the previous moment and the kinematic data at the current moment; and then calculating to obtain a kinematic secondary residual error of the camera according to the key motion increment data and the key pose transformation quantity.
In an embodiment, the historical key pose may be an initial pose of the time corresponding to the historical key image, or may be a target pose of the time corresponding to the historical key image. By comparing the historical key pose of the camera with the target pose at the current moment, the key pose change quantity generated by the adjusted pose in the interval time of the two key images can be calculated, and the key images are representative images in local time, so that the key pose change quantity determined by using the two adjacent key images can obtain more accurate camera incremental motion.
In an embodiment, the kinematic data at the previous time may refer to the kinematic data corresponding to the previous non-key image, or may refer to the kinematic data corresponding to the previous key image. The difference between the camera motion obtained by the calculation of the kinematic data (namely the camera motion actually observed) and the camera motion predicted by the motion model and the target pose is more accurately determined by comparing the numerical deviation between the kinematic data at the previous moment and the kinematic data at the current moment, namely the key motion increment data, and further by comparing the deviation between the camera increment motion obtained by the calculation of the kinematic data and the camera increment motion estimated by the target position prediction after adjustment, namely the kinematic secondary residual error is calculated by utilizing the key motion increment data and the key pose conversion amount, so that the map points corresponding to the target pose and the current moment image can be optimized to obtain more accurate key poses and key map points.
Referring to fig. 14, fig. 14 is a schematic diagram showing a practical application scenario of a data processing method for a bipedal robot according to an embodiment of the application. Kinematic data, inertial measurement data and images are acquired in the data input stage, and a historical pose at the last moment can also be acquired. Then entering a real-time detection stage, initializing a map, detecting an image shot by a camera in real time, and carrying out feature extraction and feature matching on the image at the previous moment and the image at the current moment to obtain a visual re-projection error; and meanwhile, the inertial measurement data can be subjected to integral processing to obtain an initial waist pose, the initial waist pose is obtained by calculation by utilizing the initial waist pose and the kinematic data, and then the kinematic residual error is obtained by calculating by utilizing the initial pose, the historical pose, the kinematic data at the previous moment and the kinematic data at the current moment. And performing pre-integration processing on the inertial measurement data to obtain an inertial data residual error. Then, the initial pose can be jointly optimized according to the kinematic residual error, the inertial data residual error and the visual re-projection error to obtain the target pose. In addition, when the image is detected in real time, whether the image at the current moment can be confirmed as a key image is also judged, if the image at the current moment is confirmed as the key image, local binding optimization can be carried out on the target pose at the current moment and map points corresponding to the image at the current moment, the historical key pose and map points corresponding to the historical key image, then an Inertial Measurement Unit (IMU) is initialized, accuracy of inertial measurement data is guaranteed, scale optimization is carried out continuously, and the scale refers to the proportional relation between the physical size of a real object in the current scene and the pixel distances corresponding to the real object in the image so as to establish an accurate spatial position. Therefore, the key pose and the key map point are obtained by optimizing the target pose and the map point of the key image through fusing the kinematic data, the inertial measurement data and the image, the accuracy of the key pose and the key map point can be improved, and further more accurate pose and map point can be obtained in the follow-up closed loop thread and global optimization process, so that a more accurate map can be constructed.
The following describes in detail a processing method of user generated content provided by the embodiment of the present application by using a specific example.
Referring to fig. 15, fig. 15 is a flowchart showing an overall method of processing user-generated content provided as a specific example. In fig. 15, the processing method of the user generated content may include, but is not limited to, steps 1501 to 1513.
Step 1501: and acquiring kinematic data of the biped robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment, and acquiring a last moment image and a current moment image which are shot by a camera.
In this step, the kinematic data at the previous time may include foot stress data at the previous time, lower limb motion data at the previous time, and trunk motion data at the previous time; the current moment kinematic data comprise current moment foot stress data, current moment lower limb motion data and current moment trunk motion data.
Step 1502: and acquiring the initial pose of the camera at the current moment.
In this step, the initial pose of the camera at the current moment may be obtained by calculating data (such as kinematic data and inertial measurement data) of the bipedal robot or by predicting the historical pose of the camera at the historical moment, which is not limited herein. For example, the inertial measurement data from the last moment to the current moment can be subjected to integral processing to obtain waist initial pose data, and then, according to a fourth motion chain from the waist of the bipedal robot to the camera, the trunk motion data at the current moment in the kinematic data at the current moment is subjected to data conversion to obtain trunk initial conversion data; then, according to the waist initial pose data and the trunk initial conversion data, calculating to obtain the initial pose of the camera at the current moment; for another example, the kinematic data at the current moment may include lower limb motion data at the current moment and torso motion data at the current moment, so that a pose prediction model obtained by training a historical pose of the camera at the historical moment may be utilized to predict the poses of the lower limb motion data at the current moment and the torso motion data at the current moment, so as to obtain an initial pose of the camera at the current moment. After this step is completed, step 1503 may be executed, or step 1503 and step 1510 may be executed simultaneously.
Step 1503: and determining the supporting foot at the previous moment according to the foot stress data at the previous moment, and determining the supporting foot at the current moment according to the foot stress data at the current moment.
In this step, the support state of the biped robot includes a left single-foot support state, a right single-foot support state, and a left-right simultaneous support state, and therefore, the support foot of the biped robot in the left single-foot support state is a left foot, the support foot of the biped robot in the right single-foot support state is a right foot, and the support foot of the biped robot in the left-right simultaneous support state is a left-right biped. The support foot at the previous moment and the support foot at the current moment can be the same or different.
Step 1504: and according to a first motion chain from the support foot to the waist of the bipedal robot at the previous moment, performing data conversion on the lower limb motion data at the previous moment to obtain lower limb conversion data at the previous moment.
Step 1505: and according to a second motion chain from the supporting feet to the waist of the bipedal robot at the current moment, performing data conversion on the lower limb motion data at the current moment to obtain lower limb conversion data at the current moment.
Step 1506: according to a third motion chain from the waist of the biped robot to the camera, respectively performing data conversion on trunk motion data at the previous moment and trunk motion data at the current moment to obtain trunk conversion data at the previous moment and trunk conversion data at the current moment.
Step 1507: and determining foot pose transformation data according to the data of the support foot at the previous moment and the support foot at the current moment.
Step 1508: and performing matrix transposition according to the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment to obtain pose transposition data at the previous moment.
Step 1509: multiplying the bipedal pose transformation data, the pose transposition data at the previous moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain the motion increment data of the camera.
Step 1510: and acquiring the historical pose of the camera at the last moment, and calculating according to the historical pose and the initial pose to obtain the pose change quantity of the camera.
Step 1511: and calculating to obtain the kinematic residual error of the camera according to the kinematic incremental data and the pose variation.
In this step, the motion delta data of the camera includes displacement delta data and rotation delta data, and the pose variation of the camera includes displacement variation data and rotation variation data, so that the kinematic residual may be obtained by: firstly, carrying out difference value calculation on the displacement increment data and the displacement change data to obtain a displacement change value; then carrying out logarithmic mapping according to the rotation increment data and the rotation change data to obtain an attitude change value; and then constructing a matrix according to the displacement change value and the attitude change value to obtain a kinematic residual error of the camera.
Step 1512: and carrying out pre-integration processing on the inertia measurement data to obtain an inertia data residual error, and carrying out feature extraction and feature matching processing on the previous moment image and the current moment image to obtain a visual re-projection error.
Step 1513: and calculating to obtain the target pose of the camera at the current moment according to the kinematic residual error, the inertial data residual error and the visual re-projection error.
Step 1514: determine whether the current time image is a key image? If yes, go to step 1515, if no, go to step 1519.
In this step, the key image may be confirmed by judging whether a preset detection condition is satisfied, and the preset detection condition may include one or more of the following cases: the number of the determined key images is smaller than the number of the preset images, the time interval from the last key image is larger than the preset time interval, the number of the three-dimensional points obtained after feature matching of the current moment image is larger than a preset three-dimensional point threshold value, and the like.
Step 1515: and acquiring historical kinematic data, historical inertial measurement data and historical key pose at corresponding moments of the historical key images.
In this step, the historical key image, the historical kinematic data at the time corresponding to the historical key image, the historical inertial measurement data, and the historical key pose may be stored in the robot terminal or in the server. When the historical key image, the historical kinematic data of the corresponding moment of the historical key image, the historical inertial measurement data and the historical key pose are all stored in the robot terminal, the robot terminal can send the corresponding data to the server.
Step 1516: and calculating to obtain the key motion increment data of the camera according to the kinematic data at the previous moment and the kinematic data at the current moment.
Step 1517: and calculating to obtain a kinematic secondary residual error of the camera according to the key motion increment data and the key pose transformation quantity.
Step 1518: and optimizing map points corresponding to the target pose and the current moment image according to the kinematic secondary residual error, the historical kinematic data, the historical inertial measurement data, the historical key image and the current moment image to obtain the optimized key pose and key map points.
Step 1519: ending the step flow.
In this embodiment, by the data processing method for the biped robot including the foregoing steps 1501 to 1519, by first acquiring the kinematic data of the biped robot at the last time, the kinematic data of the current time, and the inertial measurement data of the last time to the current time, the previous time image and the current time image captured by the camera disposed at the head position of the biped robot may be acquired, and in addition, the initial pose of the camera at the current time may be acquired, so that the kinematic residual error of the camera may be calculated according to the kinematic data of the last time, the kinematic data of the current time, and the initial pose, and therefore, the motion change of the biped robot at the adjacent time may be calculated, and the deviation between the data calculated by the kinematic data (i.e. the actual observed transformation amount) and the data calculated by the initial pose (i.e. the estimated transformation amount) may be determined, thereby being helpful for correcting the camera pose, and reducing the influence of the motion blur of the image. Then, the target pose of the camera at the current moment can be obtained through calculation according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image, and the initial pose is optimized and adjusted by combining the motion relation between the inertial measurement data and the kinematic data at a plurality of moments and the visual projection relation between the previous moment image and the current moment image, so that the tight coupling of multi-source information can be realized, the robustness of pose estimation can be improved, and more accurate pose data of the bipedal robot can be obtained.
The following describes application scenarios of the embodiments of the present application with some practical examples.
The data processing method for the bipedal robot provided by the embodiment of the application can be applied to different application scenes such as environment exploration of the bipedal robot and logistics distribution of the bipedal robot, and the following description will be given by taking a scene of environment exploration of the bipedal robot and a scene of logistics distribution of the bipedal robot as an example.
Scene one
The data processing method for the bipedal robot provided by the embodiment of the application can be applied to scenes of environment exploration of the bipedal robot, for example, the bipedal robot is assumed to explore in a severe environment such as an underground cave, the bipedal robot can acquire sensor data obtained by measuring sensors such as a joint encoder, a pressure sensor, an inertial measurement sensor and the like of the bipedal robot in the motion process, and images shot by a camera are returned to a robot terminal, the robot terminal can sort acquired sensor data and images, then send the acquired sensor data and images of the bipedal robot to a server, the last moment kinematic data, the current moment kinematic data and the inertial measurement data from the last moment to the current moment, and can send the last moment image and the current moment image obtained by the camera to the server, so that the server can acquire the initial pose of the camera at the current moment through the data sent by the robot terminal, then calculate the acquired kinematic data, the current moment kinematic data and the initial pose of the camera, then can calculate the images of the camera at the current moment and the initial pose according to the last moment, the current moment and the current pose, the images of the camera can be sent to the server, the environment can be acquired by the camera can be calculated, the environment can be displayed by the camera at the terminal, the current pose can be obtained by the camera, the environment can be obtained after the environment can be calculated, and the environment can be obtained by the environment can be displayed by the camera is calculated, the exploration operator can complete the environment exploration work and the map mapping work of the environment where the biped robot is located according to the environment map displayed by the robot terminal.
Scene two
The data processing method for the bipedal robot provided by the embodiment of the application can also be applied to a scene of logistics distribution of the bipedal robot, for example, the bipedal robot is assumed to transport goods through a goods warehouse, the bipedal robot can acquire sensor data obtained by measuring sensors such as a joint encoder, a pressure sensor, an inertial measurement sensor and the like of the bipedal robot in a moving process, and an image shot by a camera is returned to a robot terminal, the robot terminal can arrange acquired sensor data and images, then send the acquired sensor data, the current moment of motion data and the inertial measurement data from the previous moment of time to a server, and can also send the previous moment of motion data and the current moment of time image obtained by the camera to the server, so that the server can acquire an initial pose of the camera at the current moment of time through the data sent by the robot terminal, then the server can calculate the acquired motion data and the initial pose according to the previous moment of time of motion data, the current moment of time of motion data and the current moment of time, can calculate the camera and the current pose, can then send the camera and the current moment of time to the current moment of time to the goods warehouse, can further obtain a map of goods environment, the environment can be further displayed by the robot terminal according to the moving environment, the current position of the camera can be indicated by the robot, and the environment can be further indicated by the robot terminal, and the environment can be indicated to the environment can be displayed by the robot terminal, the logistics distribution work of the biped robot is realized.
It will be appreciated that, although the steps in the flowcharts described above are shown in order as indicated by the arrows, these steps are not necessarily performed in order as indicated by the arrows. The steps are not strictly limited in order unless explicitly stated in the present embodiment, and may be performed in other orders. Moreover, at least some of the steps in the flowcharts described above may include a plurality of steps or stages that are not necessarily performed at the same time but may be performed at different times, and the order of execution of the steps or stages is not necessarily sequential, but may be performed in turn or alternately with at least a portion of the steps or stages in other steps or other steps.
Referring to fig. 16, an embodiment of the present application further discloses a data processing apparatus for a bipedal robot, the data processing apparatus 1600 for a bipedal robot capable of implementing the data processing method for a bipedal robot in the previous embodiment, the bipedal robot having a camera at a head position, the data processing apparatus 1600 comprising:
a data acquisition unit 1610, configured to acquire kinematic data of the bipedal robot at a previous time, kinematic data of the bipedal robot at a current time, and inertial measurement data from the previous time to the current time;
An image obtaining unit 1620, configured to obtain a previous time image and a current time image obtained by capturing with a camera;
the pose acquisition unit 1630 is used for acquiring the initial pose of the camera at the current moment;
the residual calculation unit 1640 is configured to calculate a kinematic residual of the camera according to the kinematic data at the previous time, the kinematic data at the current time, and the initial pose;
the pose calculating unit 1650 is configured to calculate a target pose of the camera at the current time according to the kinematic residual error, the inertial measurement data, the previous time image, and the current time image.
In an embodiment, the residual calculation unit 1640 is further configured to:
according to the kinematic data at the previous moment and the kinematic data at the current moment, calculating to obtain kinematic increment data of the camera;
and calculating to obtain the kinematic residual error of the camera according to the motion increment data and the initial pose.
In an embodiment, the residual calculation unit 1640 is further configured to:
acquiring a historical pose of a camera at the last moment;
calculating according to the historical pose and the initial pose to obtain the pose variation of the camera;
and calculating to obtain the kinematic residual error of the camera according to the kinematic incremental data and the pose variation.
In an embodiment, the residual calculation unit 1640 is further configured to:
Performing difference calculation on the displacement increment data and the displacement change data to obtain a displacement change value;
carrying out logarithmic mapping according to the rotation increment data and the rotation change data to obtain an attitude change value;
and constructing and obtaining a kinematic residual error of the camera according to the displacement change value and the posture change value.
In an embodiment, the residual calculation unit 1640 is further configured to:
determining the supporting foot at the previous moment according to the foot stress data at the previous moment, and determining the supporting foot at the current moment according to the foot stress data at the current moment;
according to a first motion chain from the support foot to the waist of the bipedal robot at the previous moment, performing data conversion on the lower limb motion data at the previous moment to obtain lower limb conversion data at the previous moment;
according to a second motion chain from the supporting feet to the waist of the bipedal robot at the current moment, performing data conversion on the lower limb motion data at the current moment to obtain lower limb conversion data at the current moment;
according to a third motion chain from the waist of the biped robot to the camera, respectively carrying out data conversion on trunk motion data at the previous moment and trunk motion data at the current moment to obtain trunk conversion data at the previous moment and trunk conversion data at the current moment;
And calculating to obtain the motion increment data of the camera according to the lower limb conversion data at the previous moment, the lower limb conversion data at the current moment, the trunk conversion data at the previous moment and the trunk conversion data at the current moment.
In an embodiment, the residual calculation unit 1640 is further configured to:
determining foot pose transformation data according to the data of the supporting foot at the previous moment and the supporting foot at the current moment;
performing matrix transposition according to the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment to obtain pose transposition data at the previous moment;
multiplying the bipedal pose transformation data, the pose transposition data at the previous moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain the motion increment data of the camera.
In an embodiment, the pose acquisition unit 1630 is further configured to:
integrating the inertial measurement data to obtain initial waist pose data;
according to a fourth motion chain from the waist of the biped robot to the camera, carrying out data conversion on trunk motion data at the current moment to obtain trunk initial conversion data;
and calculating to obtain the initial pose of the camera at the current moment according to the initial pose data of the waist and the initial conversion data of the trunk.
In an embodiment, the pose acquisition unit 1630 is further configured to:
and predicting the pose of the lower limb movement data at the current moment and the trunk movement data at the current moment to obtain the initial pose of the camera at the current moment.
In an embodiment, the pose calculation unit 1650 is further configured to:
pre-integrating the inertial measurement data to obtain inertial data residual errors;
performing feature extraction and feature matching processing on the previous moment image and the current moment image to obtain a visual re-projection error;
and calculating to obtain the target pose of the camera at the current moment according to the kinematic residual error, the inertial data residual error and the visual re-projection error.
In an embodiment, the data processing device 1600 for a bipedal robot further comprises:
and the secondary residual calculation unit is used for calculating and obtaining the kinematic secondary residual of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the target pose if the current moment image is confirmed to be the key image.
The historical information acquisition unit is used for acquiring historical key images and historical inertial measurement data at corresponding moments of the historical key images;
and the data local optimization unit is used for optimizing the map points corresponding to the target pose and the current moment image according to the kinematic secondary residual error, the historical kinematic data, the historical inertial measurement data, the historical key image and the current moment image to obtain the optimized key pose and the key map points.
In an embodiment, the secondary residual calculation unit is further configured to:
acquiring a historical key pose at a moment corresponding to a historical key image, and calculating according to the historical key pose and a target pose to obtain a key pose conversion amount of the camera;
calculating key motion increment data of the camera according to the kinematic data at the previous moment and the kinematic data at the current moment;
and calculating to obtain a kinematic secondary residual error of the camera according to the key motion increment data and the key pose transformation quantity.
It should be noted that, since the data processing apparatus 1600 for a bipedal robot according to the present embodiment can implement the data processing method for a bipedal robot according to the previous embodiment, the data processing apparatus 1600 for a bipedal robot according to the present embodiment has the same technical principle and the same beneficial effects as the data processing method for a bipedal robot according to the previous embodiment, and in order to avoid duplication of content, no further description is given here.
Referring to fig. 17, the embodiment of the present application further discloses a data processing apparatus for a bipedal robot, the data processing apparatus 1700 for a bipedal robot comprising:
at least one processor 1701;
at least one memory 1702 for storing at least one program;
When the at least one program is executed by the at least one processor 1701, a data processing method for a bipedal robot as described above is implemented.
The embodiment of the application also discloses a computer readable storage medium, wherein a computer program executable by a processor is stored, and the computer program executable by the processor is used for realizing the data processing method for the bipedal robot.
The embodiment of the application also discloses a computer program product, which comprises a computer program or computer instructions, wherein the computer program or the computer instructions are stored in a computer readable storage medium, and a processor of the data processing device for the bipedal robot reads the computer program or the computer instructions from the computer readable storage medium, and executes the computer program or the computer instructions, so that the data processing device for the bipedal robot executes the data processing method for the bipedal robot.
The terms "first," "second," "third," "fourth," and the like in the description of the application and in the above figures, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the application described herein may be implemented, for example, in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed or inherent to such process, method, article, or apparatus.
It should be understood that in the present application, "at least one (item)" means one or more, and "a plurality" means two or more. "and/or" for describing the association relationship of the association object, the representation may have three relationships, for example, "a and/or B" may represent: only a, only B and both a and B are present, wherein a, B may be singular or plural. The character "/" generally indicates that the context-dependent object is an "or" relationship. "at least one of" or the like means any combination of these items, including any combination of single item(s) or plural items(s). For example, at least one (one) of a, b or c may represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", wherein a, b, c may be single or plural.
In the several embodiments provided in the present application, it should be understood that the disclosed system, apparatus and method may be implemented in other manners. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of the units is merely a logical function division, and there may be additional divisions when actually implemented, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be an indirect coupling or communication connection via some interfaces, devices, or units, which may be in electrical, mechanical, or other forms.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in the embodiments of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied essentially or in part or all of the technical solution or in part in the form of a software product stored in a storage medium, including instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The step numbers in the above method embodiments are set for convenience of illustration, and the order of steps is not limited in any way, and the execution order of each step in the embodiments may be adaptively adjusted according to the understanding of those skilled in the art.
Claims (15)
1. A data processing method for a bipedal robot, wherein a head position of the bipedal robot is provided with a camera, the method comprising the steps of:
acquiring kinematic data of the biped robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment;
acquiring a last moment image and a current moment image which are shot by the camera;
acquiring an initial pose of the camera at the current moment;
calculating to obtain a kinematic residual error of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose;
and calculating the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image.
2. The method according to claim 1, wherein the calculating the kinematic residual of the camera according to the kinematic data at the previous time, the kinematic data at the current time and the initial pose comprises:
According to the kinematic data at the previous moment and the kinematic data at the current moment, calculating to obtain motion increment data of the camera;
and calculating to obtain the kinematic residual error of the camera according to the motion increment data and the initial pose.
3. The method of claim 2, wherein the calculating the kinematic residual of the camera from the kinematic delta data and the initial pose comprises:
acquiring a historical pose of the camera at the last moment;
calculating to obtain the pose variation of the camera according to the historical pose and the initial pose;
and calculating to obtain the kinematic residual error of the camera according to the motion increment data and the pose variation.
4. A method according to claim 3, wherein the motion delta data comprises displacement delta data and rotation delta data, and the pose change amount comprises displacement change data and rotation change data;
the calculating to obtain the kinematic residual error of the camera according to the motion increment data and the pose variation comprises the following steps:
performing difference calculation on the displacement increment data and the displacement change data to obtain a displacement change value;
Carrying out logarithmic mapping according to the rotation increment data and the rotation change data to obtain an attitude change value;
and constructing and obtaining the kinematic residual error of the camera according to the displacement variation value and the attitude variation value.
5. The method of claim 2, wherein the last moment kinematic data includes last moment foot stress data, last moment lower limb motion data, and last moment torso motion data, and the current moment kinematic data includes current moment foot stress data, current moment lower limb motion data, and current moment torso motion data;
the calculating to obtain the motion increment data of the camera according to the kinematic data of the last moment and the kinematic data of the current moment comprises the following steps:
determining a supporting foot at the previous moment according to the foot stress data at the previous moment, and determining a supporting foot at the current moment according to the foot stress data at the current moment;
according to a first motion chain from the support foot at the previous moment to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the previous moment to obtain lower limb conversion data at the previous moment;
according to a second motion chain from the supporting foot at the current moment to the waist of the bipedal robot, performing data conversion on the lower limb motion data at the current moment to obtain lower limb conversion data at the current moment;
According to a third motion chain from the waist of the bipedal robot to the camera, respectively performing data conversion on the trunk motion data at the last moment and the trunk motion data at the current moment to obtain trunk conversion data at the last moment and trunk conversion data at the current moment;
and calculating to obtain the motion increment data of the camera according to the lower limb conversion data at the last moment, the lower limb conversion data at the current moment, the trunk conversion data at the last moment and the trunk conversion data at the current moment.
6. The method of claim 5, wherein calculating motion delta data of the camera according to the last time lower limb conversion data, the current time lower limb conversion data, the last time torso conversion data, and the current time torso conversion data comprises:
determining foot pose transformation data according to the last moment supporting foot and the current moment supporting foot data;
performing matrix transposition according to the lower limb conversion data at the previous moment and the trunk conversion data at the previous moment to obtain pose transposition data at the previous moment;
multiplying the bipedal pose transformation data, the pose transposition data at the last moment, the lower limb transformation data at the current moment and the trunk transformation data at the current moment to obtain motion increment data of the camera.
7. The method of claim 1, wherein the current moment kinematic data comprises current moment torso kinematic data, and wherein the obtaining the initial pose of the camera at the current moment comprises:
integrating the inertial measurement data to obtain initial waist pose data;
according to a fourth motion chain from the waist of the biped robot to the camera, performing data conversion on the trunk motion data at the current moment to obtain trunk initial conversion data;
and calculating to obtain the initial pose of the camera at the current moment according to the initial pose data of the waist and the initial conversion data of the trunk.
8. The method of claim 1, wherein the current moment kinematic data includes current moment lower limb motion data and current moment torso motion data, and the obtaining the initial pose of the camera at the current moment includes:
and predicting the pose of the lower limb movement data at the current moment and the trunk movement data at the current moment to obtain the initial pose of the camera at the current moment.
9. The method according to claim 1, wherein the calculating the target pose of the camera at the current time according to the kinematic residual, the inertial measurement data, the previous time image and the current time image includes:
Pre-integrating the inertial measurement data to obtain inertial data residual errors;
performing feature extraction and feature matching processing on the previous moment image and the current moment image to obtain a visual re-projection error;
and calculating to obtain the target pose of the camera at the current moment according to the kinematic residual error, the inertial data residual error and the visual re-projection error.
10. The method of claim 1, wherein the calculating the target pose of the camera at the current time based on the kinematic residual, the inertial measurement data, the previous time image, and the current time image further comprises:
if the current moment image is confirmed to be a key image, calculating to obtain a kinematic secondary residual error of the camera according to the kinematic data of the last moment, the kinematic data of the current moment and the target pose;
acquiring a historical key image and historical inertial measurement data of corresponding moments of the historical key image;
and carrying out optimization processing on map points corresponding to the target pose and the current moment image according to the kinematic secondary residual error, the inertial measurement data, the historical key image and the current moment image to obtain the optimized key pose and key map points.
11. The method according to claim 10, wherein the calculating the kinematic secondary residual of the camera according to the kinematic data at the previous time, the kinematic data at the current time and the target pose includes:
acquiring a historical key pose at a moment corresponding to the historical key image, and calculating to obtain a key pose conversion amount of the camera according to the historical key pose and the target pose;
calculating key motion increment data of the camera according to the kinematic data at the previous moment and the kinematic data at the current moment;
and calculating to obtain a kinematic secondary residual error of the camera according to the key motion increment data and the key pose transformation quantity.
12. A data processing device for a bipedal robot, wherein a head position of the bipedal robot is provided with a camera, the data processing device comprising:
the data acquisition unit is used for acquiring kinematic data of the bipedal robot at the last moment, kinematic data of the current moment and inertial measurement data from the last moment to the current moment;
the image acquisition unit is used for acquiring a last moment image and a current moment image which are shot by the camera;
The pose acquisition unit is used for acquiring the initial pose of the camera at the current moment;
the residual calculation unit is used for calculating the kinematic residual of the camera according to the kinematic data at the previous moment, the kinematic data at the current moment and the initial pose;
and the pose calculating unit is used for calculating the target pose of the camera at the current moment according to the kinematic residual error, the inertial measurement data, the previous moment image and the current moment image.
13. A data processing apparatus for a bipedal robot, comprising:
at least one processor;
at least one memory for storing at least one program;
a data processing method for a bipedal robot according to any one of claims 1 to 11 when at least one of the programs is executed by at least one of the processors.
14. A computer-readable storage medium, in which a computer program executable by a processor is stored, which computer program, when being executed by a processor, is adapted to carry out a data processing method for a bipedal robot according to any one of claims 1 to 11.
15. A computer program product comprising a computer program or computer instructions, characterized in that the computer program or the computer instructions are stored in a computer-readable storage medium, from which computer-readable storage medium a processor of a data processing device for a bipedal robot reads the computer program or the computer instructions, which processor executes the computer program or the computer instructions, so that the data processing device for a bipedal robot performs the data processing method for a bipedal robot according to any one of claims 1 to 11.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311013119.8A CN117213475A (en) | 2023-08-11 | 2023-08-11 | Data processing method and device for bipedal robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311013119.8A CN117213475A (en) | 2023-08-11 | 2023-08-11 | Data processing method and device for bipedal robot and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117213475A true CN117213475A (en) | 2023-12-12 |
Family
ID=89037846
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311013119.8A Pending CN117213475A (en) | 2023-08-11 | 2023-08-11 | Data processing method and device for bipedal robot and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117213475A (en) |
-
2023
- 2023-08-11 CN CN202311013119.8A patent/CN117213475A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110125928A (en) | A kind of binocular inertial navigation SLAM system carrying out characteristic matching based on before and after frames | |
| Howard | Real-time stereo visual odometry for autonomous ground vehicles | |
| Hu et al. | A sliding-window visual-IMU odometer based on tri-focal tensor geometry | |
| CN107193279A (en) | Robot localization and map structuring system based on monocular vision and IMU information | |
| CN109166149A (en) | A kind of positioning and three-dimensional wire-frame method for reconstructing and system of fusion binocular camera and IMU | |
| CN112219087A (en) | Pose prediction method, map construction method, movable platform and storage medium | |
| CN110717927A (en) | Indoor robot motion estimation method based on deep learning and visual inertial fusion | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN116205947B (en) | Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium | |
| CN111795686A (en) | Method for positioning and mapping mobile robot | |
| CN111354043A (en) | Three-dimensional attitude estimation method and device based on multi-sensor fusion | |
| Campos et al. | Fast and robust initialization for visual-inertial SLAM | |
| CN114234967B (en) | Six-foot robot positioning method based on multi-sensor fusion | |
| CN113899364B (en) | Positioning method and device, equipment and storage medium | |
| JP2003271975A (en) | Method of extracting plane, extractor therefor, program therefor, recording medium therefor, and robot system mounted with plane extractor | |
| CN112101160A (en) | Binocular semantic SLAM method oriented to automatic driving scene | |
| Bloesch | State estimation for legged robots-kinematics, inertial sensing, and computer vision | |
| CN115962773A (en) | Method, device and equipment for synchronous positioning and map construction of mobile robot | |
| Steder et al. | Learning maps in 3d using attitude and noisy vision sensors | |
| Li et al. | RD-VIO: Robust visual-inertial odometry for mobile augmented reality in dynamic environments | |
| CN112731503A (en) | Pose estimation method and system based on front-end tight coupling | |
| US10977810B2 (en) | Camera motion estimation | |
| CN116577801A (en) | Positioning and mapping method and system based on laser radar and IMU | |
| CN117213475A (en) | Data processing method and device for bipedal robot and storage medium | |
| CN113847907A (en) | Positioning method and device, equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |