重度拖延症失踪人口终于回归了。这一次本菜做一个看起来挺装逼的仿真---某冗余蛇形机械臂在某狭小空间内的轨迹跟踪。额，有点类似于蛇从蛇窝里溜出去，然后在外边遛个弯儿... 话不多说，直接上场景：

1. 场景

如上图所示，左侧狭窄的矩形空间是“蛇窝”，右侧内外圆柱面以及上下底面所夹的区域是窝外的活动场所。我们期望“蛇头”，也就是机械臂末端沿着黑线所示的平面圆周做匀速运动。考虑到蛇体自身的几何参数，很显然蛇头不可能完美跟踪圆周，如果只考虑跟踪误差来驱动蛇体必然是装得头破血流。因此，为了保证蛇体的安全，要在跟踪的过程中考虑碰撞避免约束。

机器人的 Modified DH 参数如下：

2. 碰撞避免约束

为了避免与环境发生碰撞，要求控制对象与障碍物的最短距离与安全距离之间满足约束  在这里环境是静态的，那么  ， 即最短距离只是机械臂关节位置的函数。这里的障碍物已经做了简化，只有平面片和圆柱面片；机械臂连杆本来就是圆柱，不需要做简化。因此，机器人与环境的最短距离计算就只包含圆柱面与平面，圆柱面与圆柱面的最短距离计算。

为了避免自碰撞，这里限制关节位置范围即可。

3. 定义NMPC问题

这里目标函数考虑末端的位置，姿态，线速度，角速度跟踪误差以及控制输入惩罚。约束包括初始状态，关节速度控制输入模型，正运动学，正微分运动学，碰撞避免约束（这里做了连杆的圆柱半径修正），状态和控制输入约束。

其中参考轨迹定义如下：

4. 仿真结果

通过仿真可以看出为了满足碰撞避免约束，末端在竖直方向的跟踪误差比较大，这一点是很容理解的。蛇形机械臂从狭小的蛇窝出来之后，必须把末端连杆往上翘或者往下翘才能给后面的连杆腾出位置。等到末端几个连杆都进来了之后，通过调整末端几个关节位置就可以实现比较好的跟踪精度。

通过这个例子，我们可以深刻体会到什么叫约束优先。NMPC是一种一定满足约束，尽量减小目标函数的控制器，很符合特种环境下安全操作的需求。

具体的关节位置，关节速度输入如下图所示，可以看出都是在规定的约束范围内。

末端位置反馈值与参考值如下图所示，可以看出在  平面内跟踪误差较小，在  方向跟踪误差较大。

欢迎大佬们继续指教本菜！

-----------------------------------------------------

本例程源文件目前暂不能公开，请谅解!

-----------------------------------------------------

做人呐，最重要的就是开心喽。能帮到别人是最大的快乐，希望本菜鸡能对小白们有所帮助，希望大佬们的批评能帮助本菜鸡成长！