5.3.3 躯干补偿轨迹计算算法