1 机械臂关节空间位置控制

(1) 机械臂各关节独立PD控制

对于机械臂系统最简单的控制策略即在机械臂运动速度不大时，可以忽略其离心力、科氏力影响以及各连杆的耦合，进而将机械臂视为解耦的线性系统，对其控制采用基于n个独立关节的控制，对每个关节施加PD控制。其控制率如下：

\tau_i=k^p_i(q^d_i-q_i)+k^d_i(\dot q^d_i-\dot q_i)

比例增益k^p 在机械臂控制系统稳定的前提下，比例增益越大，机械臂控制系统稳态精度以及快速性越好。系统的响应速度随着

k^p 的增大而增加，且稳态误差随着k_p的增加而减小，但是无法从根本上消除。但是比例系数的增加会使系统的超调，震荡次数增加，相应的调节时间也会增加，不利于系统的稳定。微分增益k^d 合适的微分项增益可以减小调节时间，且可以增大比例系数从而使稳态误差减小。但是，过大或过小的微分增益会使得系统的动态性能变差。PD控制相对于其他控制算法较易实现，且其是渐进稳定的，因此当对机械臂的动态性能要求不高时可以采用该控制算法。

(2) 基于计算力矩的机械臂控制

在前述的空间机械臂独立关节的PD控制中，其控制器设计简单，且机械臂可以到达期望的一个固定位置；但是对于时变轨迹的跟踪，一般各关节运动速度较快，机械臂耦合等因素无法用独立关节的PD控制补偿，从而会产生轨迹跟踪误差。为此，可以引入非线性补偿将机器人简化为一个易于控制的线性定常系统。

通过引入非线性状态反馈将机械臂系统转化为全局线性化系统，即：

\tau=C(q,\dot q)\dot q+G(q)+H(q)u

与机器人的动力学方程比：

H(q)u=H(q)\ddot q

由于机械臂关节空间惯量矩阵H(q) 可逆，则上式可以解耦为线性定常系统。

为了使系统稳定，对上述解耦的系统引入偏置的PD控制，其与期望的关节角加速度一起构成修正后的加速度：

u=\ddot q+K^d\dot e+K^pe

式中 e机械臂各关节角度误差.

计算力矩控制的控制率如下

\tau=H(q)(\ddot q+K^d\dot e+K^pe)+C(q,\dot q)\dot q+G(q)

考虑了机械臂的非线性项，且由偏置的PD控制增加了系统的鲁棒性，使得基于关节空间的计算力矩控制可以很好的跟踪关节空间轨迹。

(3) 关节空间位置控制仿真

机械臂期望关节角度经过关节控制器后得到机械臂各个关节的驱动力矩，对于机械臂的运动情况的仿真模块，采用第三章建立的机械臂的动力学模型。

机械臂正向动力学计算出的角加速度经过积分器得到的角度以及角速度即为机械臂运动的实际角度以及实际角速度，实际角度和角速度反馈回关节控制器实现闭环控制。机械臂关节层控制对应于实际机械臂控制系统的伺服层的位置控制模式。为了研究关节空间位置控制器对于七自由度冗余机械臂的控制性能，分别采用PD控制和计算力矩控制法，在Matlab/Simulink中搭建系统框图如下

为了更好的验证两种控制器的关节空间轨迹跟踪能力，假设各关节的轨迹均为：6sin(t),单位(deg).仿真时间10，单位s. 由于机械臂1,2关节因机械臂的运动其关节负载变化较大。因此仅列出1,2关节运动情况

由于基于独立关节的PD控制系统未考虑机械臂的耦合情况，因此当机械臂运动速度较快时其无法有效的跟踪机械臂运动。因此在轨迹跟踪问题中选择计算力矩控制器。

当机械臂搬运负载，运动速度较慢时(一般不超过1deg/s)，此时机械臂的耦合情况可以忽略，且由于的PD控制系统鲁棒性较好，控制律简单，因此可以采用PD控制控制机械臂慢速运动。

假设机械臂在完成空间点到点的运动过程中，由逆运动学计算出各个关节角期望值10、15、15、20、25、30、35，单位：deg。仿真步长设为h=0.01s,仿真时间t=50s。根据3次多项式插值进行关节空间运动规划,则机械臂的期望关节角度以及关节角速度

q(t)=q_0+3(q_f-q_0)t^{-2}_ft^2-2(q_f-q_0)t^{-3}_ft^3

由于第一关节因机械臂的运动其关节负载变化较大，其仿真曲线和实际规划曲线对比如下

由于实际关节运行速度较慢，关节之间的耦合作用较弱，因此，机械臂可以达到跟踪效果。对于机械臂的运动，非基于模型的控制器如PD无法进行快速的轨迹跟踪控制，但是基于模型的控制器虽然可以进行精确的轨迹跟踪，但是其中涉及的动力学参数与实际系统可能存在较大的差异。

2 机械臂笛卡尔空间位置控制

以冗余机械臂为例，为了更好的进行七自由度冗余机械臂的控制方法分析，验证整套控制理论的完备性，可以采用全数值仿真系统，其相对于实物实验或仿真以及半实物仿真更加简洁方便。全数值仿真系统可以很便捷的得到控制系统各部分的作用以及控制系统参数的改变对整个控制系统的影响。为此在Simulink中建立空间冗余机械臂的全数值仿真系统，并对其控制算法进行验证。

冗余机械臂的位置控制仿真系统包括操作空间、扩充任务空间的轨迹规划模块，逆运动学模块，关节空间计算力矩控制器，机械臂正向动力学模块以及正运动学模块。根据轨迹规划得到的运动学量不同，其可以分为跟踪目标位置的的位置控制、跟踪目标速度的位置控制或者为跟踪目标加速度的位置控制。本节采用基于跟踪目标速度的运动控制。为了更好的跟踪期望轨迹，关节层控制器采用计算力矩控制。冗余机械臂的位置控制系统如下

该机械臂的运动控制系统为全闭环运动控制系统，其内闭环通过反馈关节角度进行关节空间位置修正，外闭环反馈笛卡尔轨迹进行笛卡尔空间位置修正，因而其位置控制精度较高。

\dot q^d=(J^TMJ+W_v)^{-1}J^TW(\dot X+KE)

轨迹规划模块根据机械臂所需完成的任务进行相应的轨迹规划，如对于冗余机械臂完成载荷搬运过程中其主任务为将载荷搬运至规定位置。

冗余分解模块则是利用运动学目标函数参数化冗余机械臂的自运动，将笛卡尔空间轨迹直接通过雅克比矩阵转化为关节空间轨迹的逆运动模块，进一步通过阻尼最小平方法中参数的选择完成冗余机械臂的关节限位、避障规划等任务。对于基于速度级的冗余分解，其生成的是关节角速度，此时关节层控制器主要为了跟踪冗余分解生成的关节角速度。

对于关节层控制系统，计算力矩模块采用自由空间计算力矩方法，其需要准确的机器人系统的动力学参数；机械臂正向动力学模块利用前述的基于空间矢量的铰接体算法建立的机械臂模型。

根据位置控制系统的构成，在Simulink分别建立各子模块，在将自模块组合一起。基于这种模块化建模过程可以方便对程序的修改以及模块的移植性。在仿真平台中仿真基于构型控制的空间冗余机械臂的运动情况。

假设空间机械臂末端带有负载，其需要在较短时间内将负载搬运至规定位置。

机械臂末端初始位姿向量X=[-1247;-860;5560;0;0;0;2.5085]，单位：mm, deg。其中Xc=2.5085为初始臂型角，假设机械臂在额外任务—臂型角保持不变的情况下将末端载荷沿着直线搬运到X=[-747;-1360;4560;0;0;0;2.5085]。机械臂的位置控制仿真研究中，未考虑其避障任务，所以可以通过规划直线轨迹由初始位姿运动到期望姿态。通过5次多项式规划机械臂笛卡尔空间运动轨迹，其初始和终点位置的速度和加速度均是连续的，机械臂运动更加平稳.由仿真结果可以看出，当机械臂在20s后运动达到期望位姿，且在该过程中始终保持其臂型角不变，且机械臂各关节在20s后均静止，机械臂没有自运动。且可以通过合理的规划臂角进行相应的扩展任务。