	US 11,904,482 B2
	Mechanical arm calibration system and mechanical arm calibration method
Jun-Yi Jiang, Nantou County (TW); Yen-Cheng Chen, Taichung (TW); Chung-Yin Chang, Taichung (TW); Guan-Wei Su, Tainan (TW); and Qi-Zheng Yang, Taoyuan (TW)
Assigned to Industrial Technology Research Institute, Hsinchu (TW)
Filed by Industrial Technology Research Institute, Hsinchu (TW)
Filed on Apr. 1, 2021, as Appl. No. 17/219,890.
Claims priority of application No. 109146442 (TW), filed on Dec. 28, 2020.
Prior Publication US 2022/0203544 A1, Jun. 30, 2022
Int. Cl. B25J 9/16 (2006.01); B25J 19/00 (2006.01)

CPC B25J 9/1692 (2013.01) [B25J 9/1602 (2013.01); B25J 9/1653 (2013.01); B25J 9/1664 (2013.01); B25J 9/1684 (2013.01); B25J 19/0095 (2013.01)]

14 Claims

1. A mechanical arm calibration system, comprising:

a trajectory tracking device, locating a position of an end point of a mechanical arm having at least one pivot in a three-dimensional space; and

a processing device, comprising:

a connection device, connecting the mechanical arm and the trajectory tracking device;

a storage device, storing a program; and

a processor, coupled to the connection device and the storage device, and configured to load and execute the program to:

calculate an actual motion trajectory of the end point when the mechanical arm is operating by using the position located by the trajectory tracking device;

retrieve a plurality of link parameters of the mechanical arm, randomly generate a plurality of sets of particles comprising a plurality of compensation amounts for the link parameters through particle swarm optimization, import 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;

calculate 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; and

update the link parameters by the compensation amount of the particles corresponding to the group best position, wherein

the trajectory tracking device comprises:

an inertial measurement unit, detecting a displacement and an orientation of the end point of the mechanical arm in the three-dimensional space; and

an ultra-wideband positioning device, detecting an ultra-wideband signal issued from a plurality of ultra-wideband base stations around, and wherein

the processor is further configured to:

fuse the displacement and the orientation detected by the inertial measurement unit, to calculate a posture of the trajectory tracking device in the three-dimensional space;

estimate 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 calculate a position of the trajectory tracking device in the three-dimensional space;

build 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;

estimate 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 calculate a prediction error covariance and a gain value;

calculate a prediction error between a plurality of estimated values and the measured values of the next state being estimated; and

correct the estimated values using the calculated prediction error and gain value, and use the corrected estimated values as the position of the end point of the mechanical arm in the three-dimensional space.