基于动力学的六自由度机器人阻抗恒力跟踪控制

1.整个代码的控制流程图如下：

2.正逆运动学计算

略

3.动力学模型

采用拉格朗日法计算机械臂的动力学模型，其输入的是机械臂的关节角度、角速度和角加速度；其中M、C、G本别是计算的惯性力、科式力和重力项，相关部分如下：

4.RBF神经网络自适应参数调节

采用RBF自适应调节阻抗控制器参数，末端每个方向单独进行参数的调整，其中rbf的输入的是力和位置，输出的是阻抗控制器的参数，rbf原理如下：

RBF的具体实现如下：

5.阻抗控制器的实现

主要是雅可比还有动力学模型的计算，以及干扰力矩的估计，具体的控制模型如下：

其中干扰力的估计如下：

6.运行结果