Research on manipulation algorithm of HRDA based on visual UTFastSLAM

. Aiming at the SLAM problem faced by humanoid robot path planning in unknown environment, firstly, based on the analysis of feature region location and information description in the region, a feature extraction algorithm based on LPP has been researched to reduce the computational complexity of SLAM and the sensitivity to wrong data association. Secondly, aiming at the problems of phase winding and phase singularity in visual phase matching, a phase matching algorithm based on adaptive Gabor filter has been researched. By extracting the phase difference and visual difference of image pairs, there is no need to explicitly calculate the local phase, so as to achieve the best matching between the ideal phase difference and the world measured phase difference. Using unscented transform ( UT ) and selective acquisition strategy, a UTFastSLAM algorithm based on Rao-Blackwellised has been proposed, which effectively reduces the particle dilution phenomenon of particle filter ( PF ) and significantly improves the calculation accuracy and robustness of FastSLAM algorithm. Finally, the HRDA experimental platform has been constructed, and the forward and reverse kinematics models of HRDA have been established to verify the effectiveness of the algorithm proposed.


Introduction
Simultaneous Localization and Mapping (SLAM) is the key technology of humanoid robot path planning.In recent years, with the improvement of the cost performance of embedded computers and the popularity of visual sensors, the realization of humanoid robot SLAM using visual information has gradually become a hot issue of general research concern by scholars at home and abroad [1][2][3].The goal of visual SLAM is to recursively recover the position of camera motion and environmental features from visual information based on Bayesian filtering technology.The biggest advantage of visual information is abundant information and high cost performance.However, projection transformation makes depth information recovery a very difficult problem, and the three-dimensional position of feature points is very important for SLAM.Although the recovery of depth information can be realized by binocular or multi camera stereo camera, it increases the manufacturing cost.For monocular cameras, the restoration of depth information has to recover three-dimensional information from multiple two-dimensional images with the help of the motion between frames.
Davis [4] first carried out the research on monocular vision SLAM, and achieved some research results in indoor active vision SLAM and positioning, but its feature extraction and matching have strict requirements on the environment, such as there must be stable pane corners, and there must be no errors in matching.In addition, some other researchers have also proposed many new research methods and improvement means [5][6][7].Previous studies have shown that feature extraction and matching are particularly important for the SLAM of monocular vision.Once robust and stable image features are obtained, motion restoration and depth restoration are feasible.
Robust invariant features have attracted extensive attention of researchers at home and abroad because of their important application value.Among them, invariant features that do not change with the viewpoint have been deeply studied, especially Locality Preserving Projections (LPP) features, which only use the information of the local region of the target to construct the feature quantity.Because these local regions may appear discretely in different positions of the target, when extracting features independently for each region, even if the target is located in a complex environment or partially occluded, some information of the target can be obtained through local feature extraction, so as to realize target recognition in complex background, which has a wider application.LPP extraction method has become a very active research direction [8][9][10][11], involving target recognition, panoramic image mosaic, virtual reality, robot autonomous navigation and other fields.For the application of extracted features in SLAM, we must also consider the problem of feature matching and management.Especially for large-scale or texture rich scenes, the feature accumulation speed is fast.On the one hand, it improves the accuracy of SLAM estimation, on the other hand, it also brings the problem of slow feature matching.Therefore, effective feature matching and feature management methods are necessary.In addition, in order to effectively reduce or eliminate the cumulative error generated in the motion process of humanoid robot, SLAM requires to estimate the consistent trajectory and environmental landmark state of humanoid robot.At present, SLAM solutions are mainly based on Bayesian filtering framework, and typical methods mainly include: Extended Kalman Filter (EKF) based on full state and Particle Filter (PF) [12].Because the computational complexity of EKF is O(n2), it cannot be applied in large-scale environment.PF scheme decouples the whole state problem of SLAM, greatly reduces the computational complexity, and provides a more feasible method for solving large-scale environment.The fast Simultaneous Localization and Mapping (FastSLAM) based on PF proposed by Montemerlo et al. is the most representative [13][14].At present, this method has received extensive attention in SLAM.However, the traditional FastSLAM method usually requires a large number of particles.Because each particle corresponds to a landmark map, if the number of particles is too large, it will greatly increase the computational burden and storage space, and EKF needs to be used for the update of each landmark map, so there are similar problems and limitations of EKF.Therefore, aiming at the SLAM problem faced by humanoid robot path planning in unknown environment, firstly, in order to reduce the computational complexity of SLAM and the sensitivity to wrong data association, a feature extraction algorithm based on LPP has been researched.Secondly, aiming at the problems of phase winding and phase singularity in visual phase matching, a phase matching algorithm based on adaptive Gabor filter has been proposed; In order to effectively reduce the particle dilution phenomenon of PF and improve the calculation accuracy and robustness of FastSLAM algorithm, the UTFastSLAM algorithm based on Rao-Blackwellised has been researched by using unscented transformation (UT) and selective acquisition strategy.Finally, the Humanoid Robot Dual Arms (HRDA) experimental platform has been constructed, and the forward and reverse kinematics models of HRDA have been established to verify the effectiveness of the algorithm proposed.

Feature extraction algorithm based on LPP
The feature extraction algorithm based on Local Preserving Projections (LPP) mainly solves two problems: (1) Location of feature region, that is, where to extract features?(solve the where problem).( 2) Description of information in the region, that is, what feature quantity is used to describe the information in the region?(solve the what problem).The so-called feature region location refers to the construction of multi-scale differential Gaussian space based on scale space theory, the use of automatic scale selection method to locate the location of key points and determine the size of the feature region.The so-called intra domain information description means that in order to make the extracted features have good singularity, a well differentiated and deterministic feature description operator must be designed, and it has strong adaptability to the illumination, displacement, vision and noise of the image.This paper studies a new descriptor, LPP feature descriptor.The so-called LPP feature is a point feature, which is the pole not near the image edge in three consecutive Gaussian difference images.The pole in the Gaussian difference image is the pixel whose gray value is larger or smaller than its surrounding pixels.It is the pixel with the most drastic change after Gaussian blur.Therefore, it has good stability.Using the extreme value of the convolution An effective way to build I is obtained by convolution of 1 I and Gaussian function ( and iterated successively.Secondly, divide each layer of the scale space into s intervals, then 2 .In the fuzzy image stack, s + 3 images must be generated for each layer, so that the later extreme point detection can cover the whole layer.Then, s + 2 Gaussian difference images are generated by subtracting the adjacent Gaussian blurred images.After processing the first layer, resample the third Gaussian image In order to detect these extreme points, ) , , ( V y x D is sampled first, and each sampling point is compared with 8 adjacent points of the current image and 9 adjacent points of the upper and lower adjacent scales.The condition for sampling points to be extracted is that their gray values are greater than or less than all 26 adjacent points.The sampling points detected above are called candidate feature points.In order to obtain more accurate image coordinates of feature points, it is necessary to compare the sampling points with their surrounding pixels.On the one hand, it can make the data of sampling points (including position, scale, main curvature ratio, etc.) more accurate.On the other hand, it can also exclude those sampling points with small difference from adjacent points or poor quality at the edge of the image.This paper adopts the positioning method proposed by brown, which has good positioning accuracy.In this method, a three-dimensional quadratic function is used to fit the local sampling points to determine the interpolation position of the maximum value.The scale space function ) , , ( V is expanded by second-order Taylor expansion, which can be expressed as formula (2).
where, D and its partial derivative are the values at the sampling point, and ) , , ( V y x x is the offset relative to the sampling point.By calculating the partial derivative of the above function with respect to x and making it zero, the position x ˆof the extreme point can be obtained, which can be expressed as formula (3).
In this experiment, all extreme points of 02 .0 ) ( x D will be deleted.In this way, according to the function value at the extreme point x ˆ, those unstable extreme points with small difference from the neighborhood points can be excluded.According to the gray level change of the local image around the feature points, each feature point is given a constant direction.In this way, the feature points can be described with the information related to the direction, so as to obtain the invariance of image rotation.Firstly, the Gaussian blurred image l with the scale closest to the feature point is selected so that all calculations can be carried out in a scale invariant manner.At this scale, the gradient value According to the gradient direction of sampling points in a certain area around the feature points, a direction histogram can be established, which contains 36 heaps and covers the direction domain of 360 degrees.Each sampling point added to the histogram is weighted according to its gradient value and Gaussian weighted circular window.
Each peak in the direction histogram corresponds to the dominant direction of each local gradient.First, the highest peak is detected.Then, any other local peak above 80% of the highest peak here is also used to create a feature point in the corresponding direction; If there are multiple peaks with close gradient values at one location, multiple feature points with the same location and scale but different directions will be created at that location; Only about 15% of points are given multiple directions, but these points play a great role in the stability of matching.Finally, for each peak, the stack values of the three histograms closest to the peak are used for parabolic interpolation fitting to make the position of the peak more accurate.

Binocular visual feature matching algorithm
The phase matching method used in this paper has the advantages of high visual accuracy, good stability and parallel computing.It is an important research method in binocular stereo matching.Aiming at the problems of phase winding and phase singularity in the existing phase matching methods and meeting the requirements of real-time image matching, this paper studies a binocular visual feature matching algorithm to extract the phase difference of image pairs based on adaptive Gabor filter, and then calculate the parallax.There is no need to explicitly calculate the local phase, which has high operation efficiency.For any given parallax, the task of stereo matching is transformed into finding a parallax value, so that the ideal phase difference and the actual measured phase difference can achieve the best matching.The algorithm can obtain accurate parallax estimation, has high robustness, and the predicted parallax range is large.In addition, RANdom SAmple Consensus (RANSAC) constraints, epipolar geometric constraints and uniqueness constraints are used to eliminate wrong matching pairs.Therefore, it is necessary to study the basic properties and timefrequency localization properties of wavelet.For the Fourier transform Now we discuss the time-frequency localization property of the window function defined by equation (6).From the perspective of time domain, when

\
V can be expressed as follows respectively.
Equation ( 7) can be expressed in the form of convolution

\
V of the frequency window can be expressed as follows.
The time-frequency window satisfies the following equation (12).
At this time, the time width of the window is and the frequency width is u , which is independent of the selection of parameters a and b .The time-frequency window has the following characteristics: (1) when the high- frequency component needs to be detected, reduce the value of a , at this time, the time window automatically narrows, and the frequency window automatically widens.At this time, it is a high-frequency window with a narrow width and a large frequency width; (2) When detecting the low-frequency component, increase the value of a , the time window will be automatically wide, and the frequency window will be automatically narrow.At this time, it is a low-frequency window with large time and narrow frequency width.
Next, the physical significance of wavelet transform is discussed from the perspective of system response.Set the input information as ) (x f and the unit excitation response of the system as , so the output of the system satisfies equation (13).
Therefore, the continuous wavelet transform of  (2) Let Q be the ratio of the center frequency and bandwidth of the filter, that is, the quality factor, then the band-pass filters formed by the change of scaling factor a are constant Q filters.
(3) When the scaling factor a changes, the bandwidth and center frequency of the bandpass filter also change.When parameter a is smaller, the center frequency is larger and the bandwidth becomes wider; When parameter a is large, the center frequency is small and the bandwidth becomes narrow.This characteristic of wavelet transform has important application value for the local characteristic analysis of signal ) (x f . For example, where the signal changes slowly, it is mainly low-frequency components and the frequency range is relatively narrow.At this time, the band-pass filter of wavelet transform is equivalent to a large case; On the contrary, where the signal changes suddenly, it is mainly high-frequency components, and the frequency range is relatively wide.The band-pass filter of wavelet transform is equivalent to a small case.In short, when the scaling factor changes from small to large, the filtering range changes from high frequency to low frequency.Therefore, wavelet transform has zoom characteristics. (4) Linear transformation.Wavelet transform is a linear transform, which has the property of linear transform: superposition.
(   information contained in wavelet transform is not redundant, which requires that wavelets with different scales and different translations are orthogonal to each other.However, when ) , ( b a is a continuous variable, it is difficult to meet this requirement. (8) Properties of cross terms of wavelet spectra.Wavelet transform is linear and there is no cross term.However, the energy distribution function The cross term of wavelet transform only appears at

UTFastSLAM algorithm based on rao-blackwellised
Unbiased Transform (UT) is a new method to calculate the statistical characteristics of nonlinear transformation random variables.It has strong advantages in solving nonlinear and non-Gaussian problems.Because the environment perception function is nonlinear due to the complexity of the environment when creating the indoor environment map, the linear approximation of EKF in the classical algorithm may produce large errors.In this paper, UT is introduced into FastSLAM to reliably approximate the posterior distribution of the pose of the humanoid robot, which overcomes the loss problem of the traditional particle method, and greatly reduces the number of particles through effective adaptive resampling, so that a good approximate posterior distribution can be obtained with only a few particles.Firstly, the current perceptual information t z is added to the traditional proposed distribution ) , , | ( , so that the particles sampled from the prior distribution move to the posterior high probability region.The traditional method to approximate the probability is to use the Gaussian approximation generated by EKF, which can be expressed as formula (18). ) , , ( Where, J is a scale parameter and controls the distance between the sigma point and the mean t s .D is a positive definite scale parameter that controls the higher-order effects produced by the nonlinear function Step represents the posture of the humanoid robot at the j sigma point of the i-th particle at The new pose ) (i t s of the humanoid robot is sampled and the humanoid robot path When the signpost is updated, a posteriori estimation of the i-th particle and the is added to the temporary particle set ‫ܜ܁‬ together with the new humanoid robot sampling pose st(i).The update of the landmark t k depends on whether it is perceived at time t , that is, whether the scanning data obtained at time t is successfully matched with the scanning data related to the landmark t k in the map library.If the matching is successful, it will be updated, otherwise it will not be perceived, and the pose will remain unchanged.The update formula can be expressed as equation (31).
According to this approximation, the posteriori of the pose of the landmark  The particles sampled from the proposed distribution may not well approximate the posterior distribution, and the difference between them needs to be measured by an evaluation.Therefore, the definition of weight coefficient can be expressed as equation (35).
Where, the target distribution of the above formula is the posterior distribution  The above formula can be obtained by the linear approximation of the nonlinear perception function g in the perception z ௧ , and the mean ) (i t z and variance t Q can be obtained directly through UT, then the weight approximation of the ith particle can be expressed as formula (37).
Resampling can have a great impact on the performance of PF: not only low weight particles are replaced by high weight particles, but also only a limited number of necessary particles are allowed to approximate posteriori.Therefore, when the proposed distribution is quite different from the posterior distribution, resampling is very important, but resampling may also ignore some particles with high weight in the particle set, which can lead to filter divergence at worst.Since the new position particles in the FastSLAM PF are extracted from the proposed distribution motion model.If the shape of the proposed distribution is similar to that of the actual posterior distribution, the particles extracted according to the proposed distribution can well represent the posterior distribution after compensation by using the weight function.However, because the proposed distribution does not consider the current perceptual information, if the posterior distribution is located, there is a great difference between the proposed distribution and the actual posterior distribution, resulting in the divergence of PF, that is, the loss problem.That is, after several iterations, the weight of most particles tends to zero, so only a few particles really play a role in the estimation of the system state.Good particles may be ignored, and the particles will be concentrated in a small area.The proposed distribution designed in this paper integrates the current perception information and samples particles from the proposed distribution through UT, so that the particles move in the high probability region of the posterior distribution.Therefore, the particle distribution is well approximate to the posterior distribution, and UKF is used to update the road signs.For the case of nonlinear and non-Gaussian distribution, to avoid the problems caused by the linearization of nonlinear equations by EKF, while UKF uses a set of carefully selected weighted sampling points to express the statistical characteristics of the system.These samples evolve according to the real nonlinear equations without linearization.It is superior to EKF both in theory and in practical application.
(5) Behavioral decision module.The complex robot makes accurate decisions according to the owner's instructions, the pose of the humanoid robot, the environmental map and the environmental perception information, controls the movement of the humanoid robot, and ensures that it can complete a certain action and the task assigned by the owner.
(6) Dual arm control module.On the basis of solving the environment map construction and object recognition of humanoid robot based on visual UTFastSLAM algorithm, the configuration space model, WTRs, forward and reverse kinematics of HRDA are established.Double arm collision avoidance and control WTRs algorithm based on DLS-SVD method and BiRRT to complete the control function of complex objects.
(7) Main program module.The main program module is responsible for the coordination and information exchange between various modules, and transmits the environmental perception information and human control information to other modules.The environment for binocular SLAM experiment adopts multiple rooms and corridors in the experimental building.There are computer tables, bookcases and other objects in the rooms.When the humanoid robot passes through the corridor, it may be dynamically disturbed by pedestrians.The tasks of global positioning, autonomous navigation and dual arm manipulation are realized on the created map.

Vision based global positioning
Through the robust feature extraction and matching algorithm proposed in this paper, combined with the multi vision geometric relationship in computer vision, the threedimensional reconstruction algorithm is applied to solve the basic matrix and essential matrix of the projection equation, so as to obtain the relative pose between the humanoid robot and the recognition road sign, and then the humanoid robot can be located globally according to the absolute pose of the recognition road sign on the map.For the more challenging problem of humanoid robot abduction, because the posterior probability distribution of humanoid robot posture is a multi-peak distribution, it is necessary to reposition based on multi hypothesis tracking algorithm according to multiple observation information, and adopt the multi hypothesis tracking algorithm of central differential particle filter based on Gaussian mixture model, It can make the multi hypothesis posterior distribution of the pose of the humanoid robot quickly converge to the real pose of the humanoid robot, and solve the problem of relocation after abduction.

Vision based autonomous navigation
Vision based autonomous navigation can adopt vision measurement method and sub map matching method.The implementation process of visual measurement method is analysed.Through robust feature extraction and matching, the pose of humanoid robot is updated in time to improve the positioning accuracy.Effective visual servo control rules are applied to restrict the behavior of humanoid robot and ensure the real-time continuous navigation of humanoid robot.Aiming at the problem of error accumulation, a sub map matching method based on multi video frames with less computation is studied to update the pose of humanoid robot in real time.

HRDA operation task
The motion mechanism of HRDA used in this experiment is similar to the structure of human upper limbs and arms, which is mainly composed of trunk and left (right) arm.The trunk has three degrees of freedom.The joint motion the Euler angle RPY (the right superscript t represents the torso), which is the Roll(left and right swing around the R axis, abbreviated as RR), Pitch( back and forth bending and rotation around the P axis, abbreviated as Rp) and Yaw( left and right rotation around the Y axis, abbreviated as RY); The left (right) arm has seven degrees of freedom The three joints of the wrist always intersect at one point.The vector describing the terminal pose configuration of HRDA has 17 dimensions, which can be defined as . The vector describing the end pose configuration of the left (right) arm of the humanoid robot can be defined as .The origin w o of the world coordinate system w ¦ is located at the center point of the two feet.The origin of torso coordinate system t ¦ is located at the center of the buttocks.The CoA ¦ origin of the double shoulder center coordinate system is located at the center of the two shoulders.The distance between the shoulders is 2D, the trunk height is t H and the lower limb height is f H .Each coordinate system of the left (right) arm is defined as ,1,…,7).

Building HRDA forward kinematics model
HRDA direct kinematics (DK) is defined as: given the geometric parameters of each link member of humanoid robot and the configuration variables connecting each link member, find the pose of HRDA end effector relative to a specific coordinate system.In this paper, Denavit-Hartenberg (D-H) method is used to determine the pose of HRDA, establish its kinematic diagram, and form the family tree structure diagram of HRDA.Its D-H parameter is ( i The D-H method is used to establish the homogeneous transformation matrix ) , ( ) , ( 1 1   ¦ , which can be expressed as equation (43).In order to improve the operability and stability of HRDA control, this paper uses the DLS-SVD method integrating Damped Leased Square (DLS) and Singular Value Decomposition (SVD) technology to solve the numerical solution of IK (48) formula of HRDA.

HRDA software framework
The HRDA software framework designed in this paper is shown in Figure 1.Based on this framework, visual processing and motion estimation are processed in parallel and interdependent at the same time, which not only reduces the complexity of the algorithm, but also improves the utilization of resources, especially suitable for the computing processing of dual core CPU.The validity of the proposed algorithm is verified by the constructed HRDA experimental platform.

Conclusion
Based on the analysis of feature region location and information description in the region, this paper proposes a feature extraction algorithm based on LPP, which reduces the computational complexity of SLAM and the sensitivity to wrong data association.Aiming at the problems of phase winding and phase singularity in visual phase matching, a phase matching algorithm based on adaptive Gabor filter has been proposed.Using UT and selective acquisition strategy, UTFastSLAM algorithm based on Rao-Blackwellised has been proposed, which effectively reduces the particle dilution phenomenon of PF and significantly improves the calculation accuracy and robustness of FastSLAM algorithm.Finally, the HRDA experimental platform has been constructed, and the forward and reverse kinematics models of HRDA have been established to verify the effectiveness of the algorithm proposed.
The researches have been sponsored by the following scientific projects: Shenzhen basic research project (JCYJ20180307124010740), school level science and technology project (SZIIT2020KJ016, LHPY-2020007,LHPY-2020008), the seventh batch of school level education and teaching reform research and practice projects (10600-20-010201-06011).

Wireless Communication Module
from two adjacent Gaussian convolution images with a scale difference of a constant factor ۹, which can be expressed as formula(1).
described below.First, the original image is convoluted with the Gaussian function to generate the pre-processed blurred image 0 I , and then 0 I is convoluted with the Gaussian function to generate the first blurred image 1 I of the first layer.Then, 2

1 sI
at the top of the first layer stack.The resampling process is to scan the pixels of 1 s I in rows and columns, so as to obtain the first blurred image of the second layer, which is 1 / 4 of the size of the first layer.Then use the above method to generate the next layer of Gaussian blurred image and Gaussian difference image, and so on, Until the image is reduced to a certain extent.Therefore, multi-layer Gaussian blur and Gaussian difference images can be obtained, which are Gaussian blur images and Gaussian difference images.Because the local minima and maxima in the Gaussian difference image ) , , ( V y x D may have feature points with constant proportion. the difference between pixels, as shown in formula (5) below.

°°®
addition, through the time-frequency analysis of the previous window function, it can be seen that ) (t \ is essentially a band-pass system, and with the change of scaling factor a .) ( , t b a \ corresponds to a series of bandpass systems with different bandwidth and center frequency.The following physical characteristics of wavelet transform are summarized.

( 1 )
The continuous wavelet transform of signal ) band-pass filters.Parameter a in ) , ( b a f W \ reflects the bandwidth and center frequency of the band-pass filter, while parameter b reflects the time parameter of the filtered output.

( 7 )
Redundancy.The redundancy of wavelet transform is illustrated by the reconstruction kernel and the reconstruction kernel equation.That is, the wavelet transform values of each point in the ) , ( b a half plane are related.The wavelet transform value at point ) the wavelet transform value of each point in the ) , ( b a half plane.

,
the correlation between them.It is called reconstructed kernel.When 0 , the value of \ K decays rapidly, and the smaller the correlation region between them.If ) at this time, the wavelet transform values in ) , ( b a plane are not related to each other, and the ITM Web of Conferences 47, 02009 (2022) CCCAR2022 https://doi.org/10.1051/itmconf/20224702009 transform has cross terms in the case of multiple signals.If

)
zero at the same time.This is different from Wigner distribution.

2 J
) (x g .E is a parameter that controls the weight of the 0th sigma point.are commonly used optimal values.It should be noted that the weight of the 0th sigma point is different when calculating the mean and covariance, which are m w 0 and c w 0 respectively. arm and right arm , that is, l ș and r ș represent the configuration of the left and right arms respectively, which is similar below).They are three rotations of the shoulder joint (Rp, RR and RY), one rotation of the axial joint around the P axis (Rp) and three rotations of the wrist joint (Rp, RR and RY).

1 ¦
the connecting link coordinate system i ¦ at each joint of HRDA, and describe its spatial relationship with the previous connecting link coordinate system i .Since all joints of HRDA are rotating joints (three rotating joints of trunk and seven rotating joints of left and right arms respectively), the general formula of

ITM 7 ¦ r 7 ¦
Web of Conferences 47, 02009 (2022) CCCAR2022 https://doi.org/10.1051/itmconf/20224702009According to the classical chain multiplication rule in the kinematics of humanoid robot, the kinematics equation in the form of HRDA forward homogeneous coordinate transformation from w ¦ to l at the end of the left (right) arm can be expressed as equation (39).

7 ¦
the left (right) arm end effector coordinate system k relative to the left (right) shoulder coordinate system k 0

ITMĬx
Web of Conferences 47, 02009 (2022) CCCAR2022 https://doi.org/10.1051/itmconf/20224702009In equation (43), and right arms), which can be obtained by substituting the four D-H parameters ) equation in the form of HRDA homogeneous coordinate transformation.Expanding it can represent the DK ( vector of the end pose of the left (right) arm of the humanoid robot is a function or mapping relationship between the 3-dimensional configuration t ș of the trunk and the 7-dimensional configuration l ș r ș of the left (right) arm.5.3.2Establishing HRDA inverse kinematics modelHRDA inverse kinematics (IK) describes the geometric parameters of HRDA and trunk linkage mechanisms and the pose of double arm end effector relative to a specific reference coordinate system, judges whether HRDA end effector can reach the pose, and solves the rotation angle of each joint configuration to reach the desired pose.By deriving the time t on both sides of equation (44), the differential relationship (45) between HRDA configuration k Ĭ and posture k e x can be obtained.

x
is the generalized velocity of the end of HRDA in the workspace.
derivative matrix of 6x10, that is, the Jacobian matrix of HRDA.The elements in row i and column j ( r l, k represents left and right arms) can be expressed as equation (46).
is redundancy in HRDA kinematics, and singularity problems will be encountered when solving IK.If the generalized inverse ) IK differential expression (47) of HRDA can be obtained from equation (45).
, and its accuracy can reach the second-order Taylor expansion.Let L be the dimension of t s , which is mainly divided into the following steps.
2The sampling i y of the random variable y is calculated according to the . The new estimation of time t is also Gaussian.It is necessary to generate Gaussian approximation to the perception model, and use UT method ITM Web of Conferences 47, 02009 (2022) CCCAR2022 https://doi.org/10.1051/itmconf/20224702009