A bionic robot arm motion trajectory optimization control method

Abstract

The application discloses a kind of bionic mechanical arm motion trajectory optimization control method, the gyroscope in ten-axis attitude sensor, accelerometer and magnetometer are respectively to the rotary motion, linear motion and direction change of big arm, small arm, palm are captured, the data collected are handled and human model is established, end mapping method is used to carry out model mapping of human to mechanical arm;Smooth filtering is introduced in the mapping process to optimize data, eliminate physiological tremor of operator to cause acquisition sense data to produce shaking unit, so that mechanical arm mapping execution trajectory is more natural and smooth;Extended Kalman filtering is used to estimate and nonlinear compensation to somatosensory data state;According to end mapping relationship, the mapping angle after filtering is converted into mechanical arm steering gear execution angle.The application is based on ROS2 processing framework, solves the operation accuracy and flexibility of mechanical arm mainly limited in somatosensory motion capture and mechanical arm motion accuracy problem.

Description

Technical Field

[0001] This invention belongs to the field of automatic control technology, and in particular relates to a method for optimizing the motion trajectory control of a bionic robotic arm. Background Technology

[0002] A bionic robotic arm is an intelligent mechanical device integrating sensing, control, and execution. It can perceive the surrounding environment, understand human intentions, and sense information such as human posture, movement, and gravity to achieve precise control of the robotic arm's degrees of freedom, enabling collaborative work with humans and assisting them in completing tasks. The operator only needs to wear a motion-sensing exoskeleton to operate the arm, which can then collect and execute the operator's movements. This separates the operator from the operating space, ensuring the enclosure of the operating space and greatly protecting the operator's safety. It also possesses capabilities for remote diagnosis, remote surgery, and remote maintenance, significantly promoting the sharing of medical and operational resources. For work in enclosed environments, such as drug development, medical surgery, underwater maintenance, and high-temperature welding, extremely high demands are placed on the robotic arm's operational precision and the degree of coordination and smoothness with the operator's movements. Therefore, improving the operational precision and flexibility of robotic arms is a crucial means to enhance medical automation and technology sharing. Based on the current development and application performance of bionic robotic arms, it is evident that the operational precision and flexibility of robotic arms are mainly limited by motion capture capabilities and the accuracy of the robotic arm's movements.

[0003] 1. Time Delay: The time delay in real-time feedback of bionic robotic arm dynamic motion capture technology may cause the movements to feel unnatural or lose continuity. For example, the humanoid robotic arm following control method based on the end-effector workspace, published in CN113967911A, uses a search genetic algorithm for pose data optimization, which leads to a delay in data execution timing.

[0004] 2. Unstable gait: When using bionic robotic arm motion capture technology, the robotic arm may not be able to stably follow the user's movements, resulting in unsmooth movements or errors. For example, a humanoid robotic arm motion-sensing following control system and control method, published in CN106313049B, directly maps the acquired node data for execution, which can lead to problems such as unstable gait and trajectory jitter during data execution.

[0005] 3. Limited sensing accuracy: Bionic robotic arm motion capture technology usually requires the use of sensors to sense the user's movements, but the accuracy of these sensors may be limited, resulting in inaccurate motion capture. Summary of the Invention

[0006] This invention addresses the limitations of robotic arm operation accuracy and flexibility, which are mainly constrained by motion capture and movement precision issues. It provides a bionic robotic arm motion trajectory optimization control method based on the ROS2 processing framework.

[0007] The technical solution adopted in this invention is as follows:

[0008] A method for optimizing and controlling the motion trajectory of a bionic robotic arm includes the following steps:

[0009] S1, motion data acquisition: Ten-axis attitude sensors are placed on the upper arm, forearm, and palm respectively, and the rotational motion, linear motion and directional changes of the upper limb are captured by the internal gyroscope, accelerometer and magnetometer.

[0010] S2, motion data processing, includes the following steps:

[0011] S21, the accelerometer is calibrated on six sides, and the results are calculated from the data measured on the six standard surfaces:

[0012]

[0013] Among them, A x A y A z The three-axis output values ​​of the accelerometer; a x a y a z For accelerometer calibration values; a x0 a y0 a z0 K represents the zero bias value of acceleration. ax1 K ax2 K ay1 K ay2 K az1 K az2 S is the installation error coefficient; ax S ay S az The scale factor;

[0014] S22, Elliptic compensation is performed on the magnetometer. The data collected by rotating the magnetometer around the Z-axis once is fitted with a spherical model to obtain the following result:

[0015]

[0016] Among them, M x M y M z The three-axis output value of the magnetometer; m x m y m z This is the magnetometer calibration value; m x0m y0 m z0 It is a zero bias value; λ x , λ y , λ z The scale factor;

[0017] S23, take the average of multiple sets of static initial data collected by the gyroscope as the zero drift error value of the gyroscope:

[0018]

[0019] Among them, G x G y G z The three-axis output value of the magnetometer; g x g y g z This is the magnetometer calibration value; g ex g ey g ez It has zero bias.

[0020] S24, based on the accelerometer, magnetometer and gyroscope and further fused together, θ[Roll(x-axis)], φ[Pitch(y-axis)], and ψ[Yaw(z-axis)] are calculated;

[0021] S25, Euler angles for the accelerometer R = [θφψ] T Euler angles of the gyroscope Data fusion is performed using complementary filtering;

[0022] S26, data is collected and transmitted uniformly by the control board on the forearm, while the MPU6050 module is placed on the shoulder to obtain the reference angle and provide the reference DH coordinate system for human joint angle mapping;

[0023] S3, pose modeling, uses DH parameter transformation to model the human body from the data, and performs model mapping from the human body to the robotic arm based on the end-effector mapping method:

[0024] S31, aligning the rigid body coordinate system {B} with the reference coordinate system {n}, then the transformation matrix from the reference coordinate system {n} to the rigid body coordinate system {B} is... for:

[0025]

[0026]

[0027] in, The direction cosine matrix is ​​in Euler angle form;

[0028] S32 performs joint angle limiting filtering based on the rotation range of the digital servo motors mounted on the robotic arm:

[0029]

[0030] Starting from the chest reference coordinate system, the upper arm, forearm, and hand are defined as first-level, second-level, and third-level rigid bodies, respectively, namely {B1}, {B2}, and {B3}; for higher-level rigid bodies, the DH transformation matrix is:

[0031]

[0032] It can be seen that the spatial position of the x-th level coordinate relative to the reference coordinate is... have:

[0033]

[0034] It can be seen that the spatial position of the x-th level coordinate relative to the (x-1)-th level coordinate... have:

[0035]

[0036] S33, scaling the operator's upper limb operating space to overlap with the robotic arm's end-effector workspace:

[0037]

[0038] Where k is the scaling factor, r max L is the operating radius of the robotic arm's end effector, and L is the operating radius of the operator's upper limb.

[0039]

[0040] get The homogeneous transformation matrix is ​​mapped to the end effector of the robotic arm, and then the deflection angles of the rigid body ends of each stage of the robotic arm are solved.

[0041] S4, Filtering optimization, introduces smoothing filtering to optimize data, eliminates the jitter unit caused by operator physiological tremors that leads to the acquisition of somatosensory data, and makes the robotic arm's mapped execution trajectory more natural and smooth;

[0042] The data is smoothed and optimized using a first-order moving average filter.

[0043]

[0044] Where y(t) is the optimized output value; p(t) is the mean filter output value; x t is the initial data; n is the filter window length; Δ is the degree of data mutation; ε is the mutation coefficient; α is the first-order filter weighting value.

[0045] The data corrected by first-order moving average filtering eliminated some abrupt changes, ensuring the accuracy of the robotic arm mapping. Furthermore, the accuracy can be further improved by adjusting ε and α. r and α f To change the execution sensitivity;

[0046] S5, Design a motion planning algorithm:

[0047] Since the filtering of data by S2 and S3 will cause a certain transmission delay, and the data processing time interval itself will cause the robotic arm's execution state to be out of sync with the operator's pose state, a certain time compensation is required for the data.

[0048] An extended Kalman filter is used to estimate the state of the somatosensory data and perform nonlinear compensation. For the nth level skeletal rigid body, the prediction equation is:

[0049]

[0050] Its extended Kalman filter can be expressed as:

[0051]

[0052] Where, ε k ,δ k For the process noise sequence and the measurement noise sequence, P k|k Q k ,R k ∈R 3n×3n To estimate the covariance, process noise covariance, and measurement noise covariance matrices, K k For the Kalman filter gain, A k C k ∈R 3n×3n The local linear matrix of the prediction equation:

[0053]

[0054] Wherein, formulas (1) and (2) are the one-step state prediction and the prediction error covariance matrix, formula (3) is the Kalman gain, and formulas (4) and (5) are the state update amounts; thus, the estimation error can be obtained as follows:

[0055]

[0056] Set an allowable error value ρ, and judge the accuracy of the prediction by performing time series supplementation or data acquisition supplementation;

[0057] According to the end-point mapping principle in S3, the basic control quantity is... Therefore, only need to Perform a three-dimensional extended Kalman filter and then compensate for the missing time series data in S2 by interpolation.

[0058] S6, the bionic robotic arm performs a following motion:

[0059] Based on the end-effector mapping relationship described in S3, the filtered mapping angle is converted into the robotic arm servo actuator angle:

[0060]

[0061] in, T is the execution matrix of the robotic arm. x The end-effector conversion matrix transmits the robotic arm execution matrix to the robotic arm main control board via serial communication, thereby enabling the robotic arm to follow biomimetic movements.

[0062] Furthermore, the specific steps of S24 are as follows:

[0063] S241 performs data processing on the accelerometer and magnetometer:

[0064]

[0065] H x =-M y cosφ+M z sinφ

[0066] H y =M x cosθ+M y sinφsinφ+M z cosφsinθ

[0067]

[0068] Among them, a b =[a x a y a z ] T The accelerometer output is the sensor's position in any orientation within the exoskeleton's joint coordinate system; a n =[0 0 g] T Let g be the output of the accelerometer when the sensor is stationary in the natural coordinate system; g = 9.81 m / sec 2 ; M is the rotation matrix; b =[M x M y M z ] T The output of the magnetometer is the sensor's position in any orientation within the exoskeleton joint coordinate system.

[0069] S242, Perform data processing on the gyroscope:

[0070]

[0071] Among them, w b =[w x w y w z ] T C represents the gyroscope output when the sensor is in any orientation within the exoskeleton joint coordinate system; b It is a rotation matrix.

[0072] Furthermore, the specific steps of S25 are as follows:

[0073] S251 reads data from the accelerometer, magnetometer, and gyroscope, and normalizes the magnetometer and accelerometer data, converting them into unit vectors respectively.

[0074] S252, Finding the gravity vector in quaternions and geomagnetic field vector Calculation error e = e ψ +e θφ Using error correction of gyroscope data, δ = K p +K i ∫e, thus obtaining the corrected gyroscope angular velocity vector w=w g +δ;

[0075] S253, using the first-order Runge-Kutta method, q(t+T)=q(t+TΩ b (t)q(t), each iteration brings in different collected data to update the quaternion.

[0076] This invention has significant advantages:

[0077] 1. This invention employs an extended Kalman filter compensation algorithm, which greatly reduces the time delay and gait instability problems in the control of the bionic robotic arm;

[0078] 2. This invention uses a ten-axis pose sensor for high-precision data acquisition, which effectively reduces the impact of the environment on motion sensing acquisition;

[0079] 3. Under the ROS2 data processing framework, the control method of the present invention can add more control nodes, realize more and more flexible functions, and has extremely high scalability. Attached Figure Description

[0080] Figure 1 A schematic diagram of the overall process for optimizing the motion of the invented bionic robotic arm;

[0081] Figure 2 This is a schematic diagram of the hardware structure of the motion sensing acquisition circuit in this invention;

[0082] Figure 3 This is the optimized control flowchart of the present invention;

[0083] Figure 4 This is a diagram showing the overlap between the rigid body coordinate system {B} and the reference coordinate system {n} in this invention.

[0084] Figure 5 The spatial position of the x-th level coordinate relative to the reference coordinate in this invention. Detailed Implementation

[0085] The invention will now be further described with reference to the accompanying drawings.

[0086] This invention is a biomimetic robotic arm motion trajectory optimization control method. It is based on the common six-degree-of-freedom servo motor control of the robotic arm, combined with a ten-axis attitude sensor for pose capture, and performs filtering optimization and algorithm compensation on the trajectory data, thereby improving the accuracy of the mechanical system in terms of time and gait.

[0087] like Figure 1As shown, this invention consists of a robotic arm operating platform and a human operating end, which can be further divided into a somatosensory exoskeleton and a central processing system. The somatosensory exoskeleton comprises a human-like joint structure, a ten-axis attitude sensor, and data transmission hardware circuitry. The central processing system uses ROS2 as its core underlying framework to implement a visual window for information interaction. The central processing system includes a somatosensory data receiving module, a trajectory optimization processing module, a robotic arm data transmission module, and an anomaly detection and interrupt handling module. The central processing system processes the somatosensory data transmitted from the somatosensory exoskeleton and issues corresponding attitude commands to the robotic arm. Simultaneously, the robotic arm sends real-time feedback sensor data to the central processing system. The somatosensory data receiving module primarily processes the data collected by the gyroscope, accelerometer, and magnetometer in the ten-axis attitude sensor of the somatosensory exoskeleton to obtain the result of human attitude calculation. By predicting motion along the measured trajectory and iteratively updating the extended Kalman filter gain, a time-series predicted trajectory is advanced. Data supplementation is determined based on the overshoot relationship between the actual error and the threshold. The robotic arm trajectory is updated based on the effective prediction results, thus achieving point-to-point new motion trajectory planning for the robotic arm. This new trajectory is then sent to the robotic arm data transmission module for mapping processing. Upon receiving the data, the robotic arm data transmission module maps it onto the robotic arm, constructing a pose model of the human and robotic arm based on the end-joint angle workspace. This outputs the joint angles of each stage of the robotic arm's servo motors to the robotic arm, resulting in a natural and reasonable bionic following motion trajectory. The anomaly detection and interruption handling module first performs a set of preset actions on the robotic arm and conducts a test operation on the motion sensor to detect steering function and posture capture. If no anomalies are found, the process proceeds normally to the next stage; otherwise, an error is reported on the display. Simultaneously, this module performs real-time monitoring during subsequent operations to ensure normal operation.

[0088] like Figure 2 As shown, the motion sensing acquisition structure of this invention is based on an exoskeleton frame. Its specific hardware includes an STM32F407ZGT6 chip as the main control chip for data acquisition, and an external minimum system. Data acquisition and transmission are performed via an I2C bus connecting three ten-axis pose sensor IMU modules and an MPU6050 module. Data is sent to the central processing module via a Bluetooth communication module. For acquiring grip posture, a bending sensor is used to collect the grip degree. LED indicators and buttons are also integrated for interactive indication and ease of operation. The basic circuitry includes a power supply circuit, a crystal oscillator, a reset circuit, and a JTAG interface.

[0089] like Figure 3 As shown, the control method using the above-mentioned device includes the following steps:

[0090] S1, Motion Data Acquisition: Motion data is acquired using a motion-sensing exoskeleton worn on the hand, forearm, upper arm, and chest. Finger rings, in conjunction with bending sensors, enable the sensors to acquire hand grip data. Ten-axis sensors are installed on the back of the hand, forearm, and upper arm, capturing rotational, linear, and directional changes of the upper limbs through internal gyroscopes, accelerometers, and magnetometers, respectively. An MPU6050 module is installed in the chest structure to provide reference coordinate angles.

[0091] S2, motion data processing, includes the following steps:

[0092] S21, the accelerometer is calibrated on six sides, and the results are calculated from the data measured on the six standard surfaces:

[0093]

[0094] Among them, A x A y A z The three-axis output values ​​of the accelerometer; a x a y a z For accelerometer calibration values; a x0 a y0 a z0 K represents the zero bias value of acceleration. ax1 K ax2 K ay1 K ay2 K az1 K az2 S is the installation error coefficient; ax S ay S az The scale factor;

[0095] S22, Elliptic compensation is performed on the magnetometer. The data collected by rotating the magnetometer around the Z-axis once is fitted with a spherical model to obtain the following result:

[0096]

[0097] Among them, M x M y M z The three-axis output value of the magnetometer; m x m y m z This is the magnetometer calibration value; m x0 m y0 m z0 It is a zero bias value; λ x , λ y , λ z The scale factor;

[0098] S23, for the gyroscope, the systematic error generated when its zero-drift time circuit is powered on can be used to collect multiple sets of initial static data from the sensor and take the average value as the zero-drift error value of the gyroscope:

[0099]

[0100] Among them, G x G y G z The three-axis output value of the magnetometer; g x g y g z This is the magnetometer calibration value; g ex g ey g ez It has zero bias.

[0101] S24, based on the accelerometer, magnetometer, and gyroscope, and further fused together, θ[Roll(x-axis)], φ[Pitch(y-axis)], and ψ[Yaw(z-axis)] are calculated:

[0102] S241 performs data processing on the accelerometer and magnetometer:

[0103]

[0104] H x =-M y cosφ+M z sinφ

[0105] H y =M x cosθ+M y sinφsinφ+M z cosφsinθ

[0106]

[0107] Among them, a b =[a x a y a z ] T The accelerometer output is the sensor's position in any orientation within the exoskeleton's joint coordinate system; a n =[0 0 g] T Let g be the output of the accelerometer when the sensor is stationary in the natural coordinate system; g = 9.81 m / sec 2 ; M is the rotation matrix; b =[M x M y M z ] TThe output of the magnetometer is the sensor's position in any orientation within the exoskeleton joint coordinate system.

[0108] S242, Perform data processing on the gyroscope:

[0109]

[0110] Among them, w b =[w x w y w z ] T C represents the gyroscope output when the sensor is in any orientation within the exoskeleton joint coordinate system; b It is a rotation matrix.

[0111] S25, for the accelerometer, the Euler angles R = [θ φ ψ] T Euler angles of the gyroscope The specific steps for data fusion using complementary filtering are as follows:

[0112] S251 reads data from the accelerometer, magnetometer, and gyroscope, and normalizes the magnetometer and accelerometer data, converting them into unit vectors respectively.

[0113] S252, Finding the gravity vector in quaternions and geomagnetic field vector Calculation error e = e ψ +e θφ Using error correction of gyroscope data, δ = K p +K i ∫e, thus obtaining the corrected gyroscope angular velocity vector w=w g +δ;

[0114] S253, using the first-order Runge-Kutta method, q(t+T)=q(t+TΩ b (t)q(t), each iteration takes in different collected data to update the quaternion;

[0115] S26, data is collected and transmitted uniformly by the control board on the forearm, while the MPU6050 module is placed on the shoulder to obtain the reference angle, providing the reference DH coordinate system for human joint angle mapping.

[0116] S3, pose modeling;

[0117] Since the angle data obtained by S1 and S2 correspond to the offset angles of the operator's upper limb skeleton relative to the natural coordinate system, rather than the joint angles relative to the device's reference coordinate system required for the robotic arm to execute, human pose modeling is necessary. Furthermore, because the operator's limb shape differs from the robotic arm's rigid structure, the joint angles obtained from the pose modeling cannot be directly executed by the robotic arm; a mapping of related relationships is required.

[0118] The DH parameter transformation is used to model the human body from the data, and the human body to the robotic arm model is mapped according to the end-effector mapping method:

[0119] S31, align the rigid body coordinate system {B} and the reference coordinate system {n}, as follows: Figure 4 As shown, the transformation matrix from the reference coordinate system {n} to the rigid body coordinate system {B} is... for:

[0120]

[0121]

[0122] in, The direction cosine matrix is ​​in Euler angle form;

[0123] S32 performs joint angle limiting filtering based on the rotation range of the digital servo motors mounted on the robotic arm:

[0124]

[0125] Starting from the chest reference coordinate system, the upper arm, forearm, and hand are defined as first-level, second-level, and third-level rigid bodies, respectively, namely {B1}, {B2}, and {B3}; for higher-level rigid bodies, the DH transformation matrix is:

[0126]

[0127] It can be seen that the spatial position of the x-th level coordinate relative to the reference coordinate is... Yes, such as Figure 5 As shown:

[0128]

[0129] It can be seen that the spatial position of the x-th level coordinate relative to the (x-1)-th level coordinate... have:

[0130]

[0131] S33, scaling the operator's upper limb operating space to overlap with the robotic arm's end-effector workspace:

[0132]

[0133] Where k is the scaling factor, r max L is the operating radius of the robotic arm's end effector, and L is the operating radius of the operator's upper limb.

[0134]

[0135] get A homogeneous transformation matrix is ​​mapped to the end effector of the robotic arm, and then the deflection angles of the rigid body ends of each stage of the robotic arm are solved.

[0136] S4, Filtering optimization, introduces smoothing filtering to optimize data, eliminates the jitter unit caused by operator physiological tremors that leads to the acquisition of somatosensory data, and makes the robotic arm's mapped execution trajectory more natural and smooth;

[0137] The data is smoothed and optimized using a first-order moving average filter.

[0138]

[0139] Where y(t) is the optimized output value; p(t) is the mean filter output value; x t is the initial data; n is the filter window length; Δ is the degree of data mutation; ε is the mutation coefficient; α is the first-order filter weighting value.

[0140] The data corrected by first-order moving average filtering eliminated some abrupt changes, ensuring the accuracy of the robotic arm mapping. Furthermore, the accuracy can be further improved by adjusting ε and α. r and α f To change the execution sensitivity.

[0141] S5, Design a motion planning algorithm:

[0142] Since the filtering of data by S2 and S3 will cause a certain transmission delay, and the data processing time interval itself will cause the robotic arm's execution state to be out of sync with the operator's pose state, a certain time compensation is required for the data.

[0143] An extended Kalman filter is used to estimate the state of the somatosensory data and perform nonlinear compensation. For the nth level skeletal rigid body, the prediction equation is:

[0144]

[0145] Its extended Kalman filter can be expressed as:

[0146]

[0147] Where, ε k ,δ k For the process noise sequence and the measurement noise sequence, P k|k Qk ,R k ∈R 3n×3n To estimate the covariance, process noise covariance, and measurement noise covariance matrices, K k For the Kalman filter gain, A k C k ∈R 3n×3n The local linear matrix of the prediction equation:

[0148]

[0149] Wherein, formulas (1) and (2) are the one-step state prediction and the prediction error covariance matrix, formula (3) is the Kalman gain, and formulas (4) and (5) are the state update amounts; thus, the estimation error can be obtained as follows:

[0150]

[0151] Set an allowable error value ρ, and judge the accuracy of the prediction by supplementing the time series or data collection.

[0152] According to the end-point mapping principle in S3, the basic control quantity is... Therefore, only need to A three-dimensional extended Kalman filter can be performed, and interpolation compensation can be made based on one of the missing time series data in S2.

[0153] S6, the bionic robotic arm performs a following motion:

[0154] Based on the end-effector mapping relationship described in S3, the filtered mapping angle is converted into the robotic arm servo actuator angle:

[0155]

[0156] in, T is the execution matrix of the robotic arm. x The end-effector conversion matrix transmits the robotic arm execution matrix to the robotic arm main control board via serial communication, thereby enabling the robotic arm to follow biomimetic movements.

[0157] The above description is a further detailed explanation of the present invention in conjunction with specific preferred embodiments. It should not be considered that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, several simple deductions or substitutions can be made without departing from the concept of the present invention, and all such deductions or substitutions should be considered to fall within the protection scope of the present invention.

Claims

1. A method for optimizing and controlling the motion trajectory of a biomimetic robotic arm, characterized in that, Includes the following steps: S1, motion data acquisition: Ten-axis attitude sensors are placed on the upper arm, forearm, and palm respectively, and the rotational motion, linear motion, and directional changes of the upper limb are captured by the internal gyroscope, accelerometer and magnetometer respectively. S2, motion data processing, includes the following steps: S21, the accelerometer is calibrated on six sides, and the results are calculated from the data measured on the six standard surfaces: Among them, A x A y A z The three-axis output values ​​of the accelerometer; a x a y a z For accelerometer calibration values; a x0 a y0 a z0 K represents the zero bias value of acceleration. ax1 K ax2 K ay1 K ay2 K az1 K az2 S is the installation error coefficient; ax S ay S az The scale factor; S22, elliptic compensation is performed on the magnetometer. The data collected by rotating the magnetometer around the Z-axis once is fitted with a spherical model to obtain the following result: Among them, M x M y M z The three-axis output value of the magnetometer; m x m y m z This is the magnetometer calibration value; m x0 m y0 m z0 It is a zero bias value; λ x , λ y , λ z The scale factor; S23, take the average of multiple sets of static initial data collected by the gyroscope as the zero drift error value of the gyroscope: Among them, G x G y G z The three-axis output value of the magnetometer; g x g y g z This is the magnetometer calibration value; g ex g ey g ez It has zero bias. S24, based on the accelerometer, magnetometer and gyroscope and further fused together, θ[Roll(x-axis)], φ[Pitch(y-axis)], and ψ[Yaw(z-axis)] are calculated; S25, Euler angles for the accelerometer R = [θφψ] T Euler angles of the gyroscope Data fusion is performed using complementary filtering. S26, data is collected and transmitted uniformly by the control board on the forearm, while the MPU6050 module is placed on the shoulder to obtain the reference angle and provide the reference DH coordinate system for human joint angle mapping; S3, pose modeling, uses DH parameter transformation to model the human body from the data, and performs model mapping from the human body to the robotic arm based on the end-effector mapping method: S31, aligning the rigid body coordinate system {B} with the reference coordinate system {n}, then the transformation matrix from the reference coordinate system {n} to the rigid body coordinate system {B} is... for: in, The direction cosine matrix is ​​in Euler angle form; S32 performs joint angle limiting filtering based on the rotation range of the digital servo motors mounted on the robotic arm: Starting from the chest reference coordinate system, the upper arm, forearm, and hand are defined as first-level, second-level, and third-level rigid bodies, respectively, namely {B1}, {B2}, and {B3}; for higher-level rigid bodies, the DH transformation matrix is: It can be seen that the spatial position BxP of the x-th level coordinate relative to the reference coordinate is... n have: It can be seen that the spatial position of the x-th level coordinate relative to the (x-1)-th level coordinate... have: S33, scaling the operator's upper limb operating space to overlap with the robotic arm's end-effector workspace: Where k is the scaling factor, r max L is the operating radius of the robotic arm's end effector, and L is the operating radius of the operator's upper limb. get The homogeneous transformation matrix is ​​mapped to the end effector of the robotic arm, and then the deflection angles of the rigid body ends of each stage of the robotic arm are solved. S4, Filtering optimization: The first-order moving average filtering method is used to smooth and optimize the data. Where y(t) is the optimized output value; p(t) is the mean filter output value; x t ε represents the initial data; n is the filter window length; Δ represents the degree of data mutation; ε is the mutation coefficient; α is the first-order filter weighting value. S5, Design a motion planning algorithm: An extended Kalman filter is used to estimate the state of the somatosensory data and perform nonlinear compensation. For the nth level skeletal rigid body, the prediction equation is: Its extended Kalman filter can be expressed as: Where, ε k ,δ k For the process noise sequence and the measurement noise sequence, P k|k Q k ,R k ∈R 3n×3n To estimate the covariance, process noise covariance, and measurement noise covariance matrices, K k For the Kalman filter gain, A k C k ∈R 3n×3n The local linear matrix of the prediction equation: Wherein, formulas (1) and (2) are the one-step state prediction and the prediction error covariance matrix, formula (3) is the Kalman gain, and formulas (4) and (5) are the state update amounts; thus, the estimation error can be obtained as follows: Set an allowable error value ρ, and judge the accuracy of the prediction by performing time series supplementation or data acquisition supplementation; According to the end-point mapping principle in S3, the basic control quantity is... Therefore, only need to Perform a three-dimensional extended Kalman filter and then compensate for the missing time series data in S2 by interpolation. S6, the bionic robotic arm performs a following motion: Based on the end-effector mapping relationship described in S3, the filtered mapping angle is converted into the robotic arm servo actuator angle: in, T is the execution matrix of the robotic arm. x The end-effector conversion matrix transmits the robotic arm execution matrix to the robotic arm main control board via serial communication, thereby enabling the robotic arm to follow biomimetic movements.

2. The biomimetic robotic arm motion trajectory optimization control method according to claim 1, characterized in that, The specific steps of S24 are as follows: S241 performs data processing on the accelerometer and magnetometer: H x = -M y cosφ+M z sinφ H y =M x cosθ+M y sinφsinφ+M z cosφsinθ Among them, a b =[a x a y a z ] T The accelerometer output is the sensor's position in any orientation within the exoskeleton's joint coordinate system; a n =[0 0 g] T Let g be the output of the accelerometer when the sensor is stationary in the natural coordinate system; g = 9.81 m / sec 2 ; M is the rotation matrix; b =[M x M y M z ] T The output of the magnetometer is the sensor's position in any orientation within the exoskeleton joint coordinate system. S242, Perform data processing on the gyroscope: Among them, w b =[w x w y w z ] T C represents the gyroscope output when the sensor is in any orientation within the exoskeleton joint coordinate system; b It is a rotation matrix.

3. The bionic robotic arm motion trajectory optimization control method according to claim 1, characterized in that, The specific steps of S25 are as follows: S251 reads data from the accelerometer, magnetometer, and gyroscope, and normalizes the magnetometer and accelerometer data, converting them into unit vectors respectively. S252, Finding the gravity vector in quaternions and geomagnetic field vector Calculation error e = e ψ +e θφ Using error correction of gyroscope data, δ = K p +K i ∫e, thus obtaining the corrected gyroscope angular velocity vector w=w g +δ; S253, using the first-order Runge-Kutta method, q(t+T)=q(t+TΩ b (t)q(t), each iteration brings in different collected data to update the quaternion.

Images