一种纵向六自由度全自动机械臂及其控制系统和控制方法与流程

本发明涉及机械臂设备，具体地指一种纵向六自由度全自动机械臂及其控制系统和控制方法。

背景技术：

1、随着工业自动化普及与新能源汽车产业的快速发展，全自动机械臂凭借全流程无人化、高精度柔性对接、智能调度及全场景适配等优势，已成为车辆自动充电、港口物料搬运、工业柔性装配等场景的核心执行装备，对提升作业效率、降低人力成本、保障作业安全具有重要意义。

2、当前行业内主流全自动机械臂系统多采用悬挂式、固定式布局，构型主要分为高自由度通用型与低自由度专用型两类，在实际应用中均存在难以兼顾的技术缺陷，具体如下：高自由度机械臂关节数量过多，导致机械结构繁杂、控制难度大、制造成本居高不下；低自由度机械臂作业灵活性不足，无法适应多变工作环境与复杂姿态调整需求，应用场景受限严重。现有机械臂未针对纵向长行程、悬臂式专属构型进行优化设计，忽略悬臂臂体重力下垂变形、长行程误差累积等问题，无法有效融合视觉、力传感信息实现精准轨迹规划与跟踪，作业定位误差大，难以稳定完成充电枪插接、精密对接等高精度任务。多数机械臂无折叠收缩结构，非作业状态下空间占用大，在停车场、地下车库等狭长受限场景的工程适配性差；且收缩后无防护设计，臂体与末端执行器易发生碰撞磕碰，设备损坏风险高、耐久性差。通用控制算法未适配“直线运动与旋转关节”的串联构型，缺乏直线运动与关节主从协同、狭长空间分区规划、悬臂变形补偿等专属策略，无法兼顾长行程作业效率与近端精操作精度，同时缺少分级安全防护、故障容错与折叠收缩控制机制，无人值守工况下的可靠性不足。

3、上述技术缺陷严重制约了全自动机械臂在车辆自动充电、受限空间搬运等场景的产业化落地，研发一种关节精简、结构简单、成本低廉、适配多场景、收缩后具备防护性的纵向专用全自动机械臂系统，已成为当前行业亟待解决的关键技术问题。

技术实现思路

1、本发明的目的就是要解决上述背景技术的不足，提供一种关节精简、结构简单、成本低廉、适配多场景、收缩后具备防护性的纵向六自由度全自动机械臂及其控制系统和控制方法。

2、为实现此目的，本发明所设计的纵向六自由度全自动机械臂，包括直线运动组件和安装于直线运动组件上的机械臂组件，所述直线运动组件包括可沿直线方向往复运动的直线运动部和驱动所述直线运动部沿直线方向往复运动的直线驱动部，所述机械臂组件包括多根首尾端部通过旋转关节连接的连杆，所述旋转关节可驱动所述连杆绕连接点转动，多根所述连杆和多个所述旋转关节组成连杆机构，所述连杆机构的一端通过旋转关节连接于所述直线运动部上，所述连杆机构的另一端通过旋转关节连接设备组件；所述直线驱动部通过空间运动学建模策略、分区式轨迹规划策略和自适应闭环控制策略对所述直线运动部进行直线运动控制，所述旋转关节通过空间运动学建模策略、分区式轨迹规划策略和自适应闭环控制策略驱动所述连杆机构、所述连杆和所述设备组件绕各自的连接点转动，所述直线运动组件和所述机械臂组件通过全空间安全防护策略进行安全防护。

3、进一步的，一种基于所述的纵向六自由度全自动机械臂的控制系统，包括：空间运动学建模模块，用于对由一个直线平移自由度和五个旋转自由度组成的纵向六自由度机械臂构型进行建模，构建融合悬臂弹性变形补偿的运动学模型，通过融合悬臂弹性变形补偿的运动学模型求解末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据；分区式轨迹规划模块，用于基于所述末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据，结合狭长作业空间分区规则、作业目标位姿与作业阶段控制要求，生成机械臂各自由度纵向递进式连续运动轨迹与多段平滑融合策略，输出机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数；自适应闭环控制模块，用于基于所述机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数，结合视觉、力觉、位置及电流的实时传感反馈数据，对机械臂实施主从协同闭环控制与自适应鲁棒修正，使机械臂完成目标作业动作，并实时反馈机械臂末端位姿与机械臂运动状态数据。

4、进一步的，系统还包括全空间安全防护模块，用于基于所述关节限位及奇异位形约束数据、所述机械臂运动状态数据和多传感监测信息，对机械臂进行分级碰撞检测、故障识别操作和纵向折叠式主动容错控制，输出安全干预指令，对机械臂运动轨迹进行实时约束与闭环控制。

5、进一步的，所述对由一个直线平移自由度和五个旋转自由度组成的纵向六自由度机械臂构型进行建模，构建融合悬臂弹性变形补偿的运动学模型的方法包括：建立包含基坐标系、直线运动坐标系、关节坐标系及末端工具坐标系的运动学坐标体系；定义系统广义关节变量为：，其中，为直线运动位移，为第i个旋转关节角度；构建直线运动组件的平移自由度的齐次变换矩阵：；构建各旋转关节的齐次变换矩阵：

6、；其中，为连杆长度，为连杆扭角，为连杆偏距；建立分段式连杆挠曲变形模型，计算各级连杆的挠度与转角：，，其中，为第i段连杆等效载荷，为等效弯矩，为杆长，为弹性模量，为截面惯性矩；生成机械臂末端变形补偿向量：；其中，为第段连杆在x向的挠度；为第段连杆在y向的挠度，为第段连杆在z向的挠度，为第段连杆在x向转角，为第段连杆在y向转角，为第段连杆在z向转角，得到融合变形补偿的运动学模型：，其中，为基坐标系到工具坐标系的变换矩阵，为直线运动部到1号旋转关节的齐次坐标变换矩阵，为1号旋转关节到2号旋转关节的齐次坐标变换矩阵，为2号旋转关节到3号旋转关节的齐次坐标变换矩阵，为3号旋转关节到4号旋转关节的齐次坐标变换矩阵，为4号旋转关节到5号旋转关节的齐次坐标变换矩阵，为5号旋转关节到工具坐标系的齐次坐标变换矩阵。

7、进一步的，所述通过融合悬臂弹性变形补偿的运动学模型求解末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据的方法包括：定义机械臂末端六维位姿向量：，其中，为机械臂末端位姿向量；为机械臂末端位置坐标；为机械臂末端姿态角；从总齐次变换矩阵中拆分出机械臂末端位置向量与旋转矩阵，解算姿态角：

8、；；；其中，为基坐标系到机械臂末端工具坐标系的旋转矩阵元素，结合悬臂变形补偿量，得到补偿后末端实际位姿：，其中，为关节变量至末端位姿的刚体运动学映射函数，为连杆变形补偿量；根据机械臂目标末端位姿反解关节目标变量，反解公式为：，其中，为机械臂目标末端位姿，为正运动学函数的逆映射；分配直线运动纵向位移：，其中，为直线运动目标位移，为机械臂末端目标x轴坐标，为连杆相对直线运动方向的夹角，m为参与纵向投影的连杆数量；采用余弦定理求解机械臂前端旋转关节角度，结合姿态约束完成剩余关节变量修正；在刚体雅克比矩阵基础上叠加变形补偿项，得到自适应雅克比矩阵：；其中，为变形自适应雅克比矩阵，为理想刚体雅克比矩阵，为变形补偿修正项，为估计的载荷、姿态等参数集合；采用阻尼伪逆避免矩阵奇异，公式为：；其中，为雅克比矩阵阻尼伪逆，为阻尼因子，为单位矩阵，t为矩阵倒置；读取机械臂硬件参数，设定关节角度、速度、加速度、直线运动行程的上下限，得到关节限位数据；计算奇异性指标判定奇异位形，公式为：；其中，为奇异性指标，为矩阵行列式，当时，判定为奇异位形，生成奇异位形约束数据，其中，为奇异判定阈值。

9、进一步的，所述基于所述末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据，结合狭长作业空间分区规则、作业目标位姿与作业阶段控制要求，生成机械臂各自由度纵向递进式连续运动轨迹与多段平滑融合策略，输出机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数的方法包括：定义位置距离判别函数与姿态误差判别函数：，；其中，为机械臂末端当前位置到目标位置的直线距离，为机械臂末端当前姿态与目标姿态的姿态误差，和分别为机械臂末端当前位置何机械臂末端目标位置，和分别为机械臂末端当前姿态和机械臂末端目标姿态；基于距离阈值、姿态误差阈值划分六大作业区域，分区判定函数：

10、；其中，~分别对应待机收缩区、长行程接近区、近端粗对准区、精对准区、低速插接区、安全退出区；、、、为距离阈值，＞＞＞；为姿态误差阈值；采用s型加减速规划直线运动主行程加速度：；其中，为直线运动实时加速度，为直线运动最大加速度，为加加速度作用时间，为加速段总时长；生成旋转关节随动轨迹：；其中，为关节实时轨迹；和分别为关节初始角度和关节目标角度；为0∼1区间的归一化轨迹进度函数；融合视觉修正、变形补偿，得到末端综合参考轨迹，生成各自由度初始轨迹指令；采用五次多项式实现不同区域轨迹的平滑过渡，过渡轨迹公式：；其中，为轨迹融合段函数；∼为五次多项式系数；t为过渡时长；约束切换点位置、速度、加速度连续，消除轨迹切换冲击；结合关节限位、奇异位形约束，构建综合安全代价函数优化轨迹：；其中，为安全优化目标函数；∼为权重系数；为奇异位形惩罚项；为障碍物碰撞惩罚项；输出目标轨迹指令、分区控制参数、轨迹安全约束参数。

11、进一步的，所述基于所述机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数，结合视觉、力觉、位置及电流的实时传感反馈数据，对机械臂实施主从协同闭环控制与自适应鲁棒修正，使机械臂完成目标作业动作，并实时反馈机械臂末端位姿与机械臂运动状态数据的方法包括：定义直线运动位移误差和速度误差：，，其中，为直线运动位移误差，为直线运动速度误差，，为直线运动实际位移，为直线运动目标速度，为直线运动实际速度；构建叠加摩擦和偏摆补偿的直线运动pid控制律：；，其中，为直线驱动部输出，、和为pid控制参数，、和分别为摩擦、直线度、长行程偏摆补偿项，为库仑摩擦系数，为黏性摩擦系数，为符号函数，定义单关节角度误差；构建叠加重力和摩擦补偿的关节扭矩控制律，关节摩擦补偿项为；其中，为关节扭矩输出，为重力补偿项，和为关节摩擦参数；基于机械臂末端位姿误差修正关节指令，，，其中，为机械臂末端位姿误差，为修正量，为原始参考轨迹，为修正后控制指令；总控制输出由基础项、自适应项和鲁棒项组成，，基础项，自适应项，参数更新律，鲁棒项，其中，和为反馈增益，为回归矩阵，为在线估计参数，为自适应增益，为鲁棒系数，为饱和函数，为边界层宽度；采用加权融合整合多通道传感反馈数据，为融合估计结果，分别为视觉、力传感、电流、位置反馈量，分别为各通道权重；通过判定式判断作业是否完成，通过判定式判断是否作业异常，全程采集并输出机械臂末端位姿和整机运动状态数据。

12、进一步的，所述基于所述关节限位及奇异位形约束数据、所述机械臂运动状态数据和多传感监测信息，对机械臂进行分级碰撞检测、故障识别操作和纵向折叠式主动容错控制，输出安全干预指令，对机械臂运动轨迹进行实时约束与闭环控制的方法包括：构建折叠收缩优化目标函数，约束条件为、；其中，为折叠优化目标函数，为权重系数，为机械臂折叠后包络长度，为折叠目标关节向量，为回撤路径长度，为机械臂与障碍物距离，为安全距离阈值，为设备组件保护区域；构建综合碰撞风险指标；；其中，为综合碰撞风险值，、、和分别为位置、电流、受力、位姿误差风险项，依据阈值对进行划分，将划分为一级预警、二级检测、三级应急等级并对应输出减速、停机、紧急收缩干预指令。

13、进一步的，所述基于所述关节限位及奇异位形约束数据、所述机械臂运动状态数据和多传感监测信息，对机械臂进行分级碰撞检测、故障识别操作和纵向折叠式主动容错控制，输出安全干预指令，对机械臂运动轨迹进行实时约束与闭环控制的方法还包括：定义系统健康状态向量与综合健康度：，，其中，为健康状态向量，依次表征视觉、力传感、电流、位置、执行机构工作状态，综合健康度，为加权系数，时判定系统故障；故障容错控制律为，参数在线自整定公式，其中，为容错控制输出，为容错增益，为安全目标位姿，为学习率，和分别为实际观测值和模型预测值，为回归特征向量；结合关节限位、奇异约束、安全干预指令实时修正机械臂运动轨迹。

14、还进一步的，一种所述的纵向六自由度全自动机械臂的控制系统的控制方法，包括：对由一个直线平移自由度和五个旋转自由度组成的纵向六自由度机械臂构型进行建模，构建融合悬臂弹性变形补偿的运动学模型，通过融合悬臂弹性变形补偿的运动学模型求解末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据；基于所述末端空间位姿、关节目标变量、连杆变形补偿量、关节限位及奇异位形约束数据，结合狭长作业空间分区规则、作业目标位姿与作业阶段控制要求，生成机械臂各自由度纵向递进式连续运动轨迹与多段平滑融合策略，输出机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数；基于所述机械臂各自由度的目标轨迹指令、分区控制参数和轨迹安全约束参数，结合视觉、力觉、位置及电流的实时传感反馈数据，对机械臂实施主从协同闭环控制与自适应鲁棒修正，使机械臂完成目标作业动作，并实时反馈机械臂末端位姿与机械臂运动状态数据。

15、上述空间运动学建模策略与空间运动学建模模块对应，分区式轨迹规划策略与分区式轨迹规划模块对应，自适应闭环控制策略与自适应闭环控制模块对应，全空间安全防护策略与全空间安全防护模块对应，即各策略分别与各模块的数据处理方法相同且一一对应，从而实现控制系统对纵向六自由度全自动机械臂的安全闭环控制。

16、本发明的有益效果是：构型精简可控，兼顾成本与通用性：本发明采用“一个直线平移自由度与五个旋转自由度”的纵向六自由度串联构型，相较于高自由度通用机械臂精简了关节数量，简化了机械结构与控制系统复杂度，有效降低了设备制造成本与维护成本；末端执行组件可快速更换，兼容车辆自动充电、港口物料搬运、工业柔性装配等多类作业场景，解决了传统方案中自由度与成本、通用性难以平衡的技术问题。

17、悬臂变形精准补偿，末端作业精度高：本发明建立融合悬臂弹性变形补偿的空间运动学模型，通过分段式连杆挠曲变形模型实时计算各级连杆的挠度与转角，对臂体自重下垂、负载形变引发的末端位姿误差进行前馈修正；配合变形自适应雅克比矩阵与阻尼伪逆算法规避奇异位形，有效抵消长行程误差累积，显著提升末端定位精度与姿态控制稳定性，保障充电插接、精密对接等高精度作业的可靠执行。

18、分区递进式轨迹规划，适配狭长作业场景：针对纵向长行程的狭长作业空间，采用分区式轨迹规划策略，将作业过程划分为待机收缩、长行程接近、近端粗对准、精对准、低速插接、安全退出六大功能区域，不同区域匹配差异化的速度、精度控制策略；长行程阶段以直线导轨为主驱动保障作业效率，近端阶段侧重关节姿态精调保障对接精度，并通过五次多项式实现多段轨迹的平滑过渡，消除区域切换的运动冲击，兼顾作业效率与运动平稳性。

19、主从协同闭环控制，系统鲁棒性强：构建“导轨主驱动+关节从随动”的分层协同控制架构，底层控制叠加摩擦补偿、重力补偿、长行程偏摆补偿，抑制低速爬行与姿态漂移；上层控制融合视觉、力觉、位置、电流多传感反馈数据，结合自适应参数估计与鲁棒修正算法，动态修正关节控制指令，可自适应负载变化与外部扰动，有效提升多轴同步精度与系统抗干扰能力。

20、纵向折叠收纳设计，空间适配性优异：配备纵向折叠式收缩控制策略，非作业状态或异常工况下，导轨回退至安全位置、各旋转关节依次收拢，大幅缩小设备纵向包络尺寸，适配地下车库、停车场、狭窄工位等空间受限的安装场景；收缩后末端执行器进入保护区域，可有效减少非作业状态下的磕碰损坏风险，提升设备使用寿命。

21、全域分级安全防护，无人值守可靠性高：建立全空间安全防护体系，融合位置边界、电机电流、末端受力、位姿误差构建综合碰撞风险指标，实现一级预警减速、二级检测停机、三级应急收缩的分级干预机制；配备全链路故障容错控制，传感器或执行机构出现局部故障时可执行降级控制与安全回撤，保障无人值守工况下的设备安全与作业可靠性，降低事故与损坏风险。