En el for en lugar de empezar desde 1 y comparar con $total_paginas, usa 2 variables $paginacion_inicio y $paginacion_final cuyos valores calculas en funcion de $pagina y $total_paginas. Por ejemplo:
Código PHP:
$paginacion_inicio=$pagina-3;
if (1>$paginacion_inicio) $paginacion_inicio=1;
$paginacion_final=$paginacion_inicio+5;
if ($total_paginas<$paginacion_final) $paginacion_final=$total_paginas;
if (5>($paginacion_final-$paginacion_inicio)) {
$paginacion_inicio=$paginacion_final-5;
if (1>$paginacion_inicio) $paginacion_inicio=1;
}