Patent ID: 11904482
Assignee: INDUSTRIAL TECHNOLOGY RESEARCH INSTITUTE
Field: Handling (Mechanical engineering)
Classification: CPC B  G | IPC B

Claim 7:
8. A mechanical arm calibration method, adapted for a processing device connected to a trajectory tracking device, the mechanical arm calibration method comprising:
locating a position of an end point of a mechanical arm having at least one pivot in a three-dimensional space by the trajectory tracking device, to calculate an actual motion trajectory of the end point when the mechanical arm is operating;
retrieving a plurality of link parameters of the mechanical arm, randomly generating a plurality of sets of particles comprising a plurality of compensation amounts of the link parameters through particle swarm optimization, importing the compensation amounts of each of the sets of particles into forward kinematics after addition of the corresponding link parameters, to calculate an adaptive motion trajectory of the end point,
calculating a plurality of position errors between the adaptive motion trajectory and the actual motion trajectory of each of the sets of particles for calculating a fitness value of the particle swarm optimization, to estimate a group best position in the sets of particles;
updating the link parameters by the compensation amount of the particles corresponding to the group best position; and
controlling the mechanical arm based on the updated link parameters, wherein
the trajectory tracking device comprises an inertial measurement unit configured to detect a displacement and an orientation of the end point of the mechanical arm in the three-dimensional space, and an ultra-wideband positioning device configured to detect an ultra-wideband signal issued from a plurality of ultra-wideband base stations around, and
the locating the position of the end point of the mechanical arm having at least one pivot in the three-dimensional space by the trajectory tracking device comprises:
fusing the detected displacement and orientation to calculate a posture of the trajectory tracking device in the three-dimensional space;
estimating a distance between the trajectory tracking device and each of the ultra-wideband base stations according to a strength of the ultra-wideband signal, and accordingly calculating a position of the trajectory tracking device in the three-dimensional space;
building a system model based on a Kalman filter, the system model denoting a motion state and an observed measurement state of the mechanical arm in the three-dimensional space;
estimating a next state using the system model by taking a plurality of measured values measured by the inertial measurement unit and the ultra-wideband positioning device as a current state, and calculating a prediction error covariance and a gain value;
calculating a prediction error between a plurality of estimated values and the measured values of the next state being estimated; and
correcting the estimated values using the calculated prediction error and gain value, and using the corrected estimated values as the position of the end point of the mechanical arm in the three-dimensional space.