机器人中的数值优化（十五）——PHR增广拉格朗日乘子法