CN113602480A - 一种搭载机械手的无人机自主平衡控制方法 - Google Patents

Abstract

本申请涉及一种搭载机械手的无人机自主平衡控制方法，通过改变无人机旋翼转速可实现无人机机械手组合体重心的动态调整，以防机械手工作时导致的无人机失稳。在正常使用时，它可以实现无人机重心的主动平衡，相较于目前在已造成机身倾侧后陀螺仪传感工作才能完成无人机平衡的被动技术，本发明由于其实时的对应重心调节功能，可降低飞控系统平衡机身的负担，增强无人机的飞控性能。若已知机械手的工作路径，则可计算出无人机旋翼转速调节量，实时保持无人机稳定。

Description

一种搭载机械手的无人机自主平衡控制方法

技术领域

本申请涉及一种无人机应用领域，具体涉及一种搭载机械手的无人机自主平衡控制方法。

背景技术

伴随着无人机技术的快速发展普及，应用场景不断拓宽，广泛用于工业巡检、航拍摄影、农业植保，国防安保等领域。同时，无人机当前所能承受的载荷上限不断提高，其作为融合具体功能的平台，经常会面临载荷大小、重量、形状变化的情况，无人机搭载云台相机时候，相机转动使得重心位置偏移；无人机运送快递时，运送货物载重情况各异，造成重量分布不均衡。

无人机的重量分布常常是对称的，但是载荷的重量分布却总不对称，使得无人机的重心位置脱离旋翼升力的位置，进而使得无人机角度偏斜，使无人机无法悬停，产生颠簸。搭载机械手的无人机在机械手操作过程中，会改变无人机系统整体的重心位置，造成重心偏移和不断变化，导致无人机机翼控制系统难以实时作出调整。过重、不对称且实时变化的载荷是造成无人机飞行失去稳定的诱因，若不能够有效解决机械手动作导致的无人机重心偏移和变化，无人机机翼控制系统将不能维持机身平衡并且极有可能因为一些小扰动而发生倾覆坠毁。目前，消除重心不对称的方法是通过起飞前临时调整好配重载荷安装位置，这种方法耗时较长，不够精确，并且是针对重心偏移位置固定不变化，提前知晓重心偏移量的情况，针对机械手重心不断变化的场景并不适用。

发明内容

本申请通过实时计算无人机和机械手的重心位置，动态调整重心位置来实现无人机的自平衡，在搭载的机械手操作过程中维持机身稳定平衡。

一种搭载机械手的无人机自主平衡控制方法包括如下步骤：

S1.系统以无人机机身本体作为基点确立水平面坐标系及坐标系原点，根据机械手每个运动单体的重心(x₁,y₁)，(x₂,y₂)，(x₃,y₃)，……，确定初始机械手组合体重心坐标(x_c1,y_c1)，确定初始无人机本体重心坐标(x_c2,y_c2)，得到机械手无人机组合体重心坐标(x_c,y_c)；

S2.当无人机机械手控制系统准备向机械手各关节下达运动指令时，根据该项运动指令计算得到各运动单体在关节执行完运动指令后的重心(x′₁,y′₁)，(x′₂,y′₂)，(x′₃,y′₃)，……，确定机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)，计算得到机械手重心偏离量(Δx₁,Δy₁)，进而得到机械手无人机组合体重心偏离量(Δx,Δy)；

S3.根据机械手无人机组合体重心偏离量(Δx,Δy)计算得到无人机机翼调整量，根据机翼数量决定每个机翼的调整分配量；

S4.在无人机机械手控制系统向机械手各关节下达运动指令时，无人机本体控制系统中的重心计算控制单元同步向无人机机翼电机下达机翼分配量的指令，当无人机重心靠近某个旋翼时，则该旋翼增大转速，提升该旋翼升力增大，完成在机械手操作过程中的重心稳定和平衡。

其中，S1中所述的初始机械手组合体重心坐标(x_c1,y_c1)的计算公式如下：

式中，m_i为机械手各单体的质量，(x_i,y_i)为其相应的重心坐标值。

其中，S1中所述的机械手无人机组合体重心坐标(x_c,y_c)的计算公式如下：

式中，m′为机械手总的质量，m″为无人机本体的质量。

其中，S2中所述的机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)的计算公式如下：

其中，S2中所述的机械手重心偏离量(Δx₁,Δy₁)的计算公式如下：

Δx₁＝x′_c1-x_c1,Δy₁＝y′_c1-y_c1。

其中，S2中所述的机械手无人机组合体重心偏离量(Δx,Δy)的计算公式如下：

其中，S4中所述的机翼分配量的计算公式如下：

F＝b·ω²

式中，b为四旋翼无人机的升力参数，ω为旋翼的转速。

附图说明

图1是本申请实施例一自主平衡系统框架图；

图2是本申请实施例一自主平衡系统机械结构图；

图3是本申请实施例二自主平衡系统框架图；

图4是本申请实施例二自主平衡系统机械结构图；

图5是本申请重心计算控制中心重心坐标计算示意图；

图6是本申请无人机-机械手重心计算驱动过程示意图一；

图7是本申请无人机-机械手重心计算驱动过程示意图二。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。

如图1和图2所示，实施例一中，一种搭载机械手的无人机自主平衡控制系统包括重心计算控制中心1、移动模块一2、移动模块二3、移动模块三4、导轨槽一5、导轨槽二6、导轨槽三7、齿条一8、齿条二9、齿条三10、微型电机一11、微型电机二12、微型电机三13、配重14、定位挡板一15、定位挡板二16、定位挡板三17、定位挡板四18。其中，移动模块一2设置在导轨槽一5上可沿槽运动，移动模块三4设置在导轨槽三7上可沿槽运动，导轨槽二6的两端分别与移动模块一2和移动模块三4固定连接，移动模块二3设置在导轨槽二6上可沿槽运动；齿条一8设置在导轨槽一5上，齿条二9设置在导轨槽二6上，齿条三10设置在导轨槽三7上；微型电机一11设置在移动模块一2上，并且微型电机一11的转轴驱动齿条一8的动作；微型电机二12设置在移动模块二3上，并且微型电机二12的转轴驱动齿条二9的动作；微型电机三13设置在移动模块三4上，并且微型电机三13的转轴驱动齿条三10的动作；配重14固设在移动模块二3的下端；其中，重心计算控制中心1、微型电机一11、微型电机二12和微型电机三13利用无人机外接充电回路通过无人机内部电源供电，正极导线和负极导线分别布置于导轨槽一5、导轨槽二6和导轨槽三7上；定位挡板一15和定位挡板二16设置在导轨槽一5两端；定位挡板二16和定位挡板三17设置在导轨槽三7两端。所述的定位挡板一15、定位挡板二16、定位挡板三17、定位挡板四18属于限位装置，防止导轨槽二6向导轨槽一5和导轨槽三7外运动，从而限制了可动配重的运动区间。

所述的一种搭载机械手的无人机自主平衡控制系统可以通过固定连接机构安装在无人机上。

所述的重心计算控制中心1包括整体重心位置计算分析单元和微型电机控制单元。其中，整体重心位置计算分析单元通过机械运动分析或实时状态感知掌握机械手和配重14的重心位置，并求出无人机组合体在机械手动作过程中整体重心偏移情况，根据偏移情况求得配重14所需移动的距离、方向以及最终应在的坐标。

所述的重心计算控制中心1与微型电机一11、微型电机二12和微型电机三13建立信号连接，即重心计算控制中心1中的微型电机控制单元给微型电机一11、微型电机二12和微型电机三13发送驱动指令，驱动微型电机一11、微型电机二12和微型电机三13工作。其中，微型电机控制单元发送给微型电机一11和微型电机三13的驱动指令相同，使微型电机一11和微型电机三13保持转向和转速一致，进而使导轨槽二6无转矩地平行运动，微型电机一11和微型电机三13工作后，通过齿条一8和齿条三10带动导轨槽二6及设置在导轨槽二6上的移动模块二3、齿条二9、微型电机二12和配重14做y轴运动；微型电机控制单元发送给微型电机二12驱动指令后，微型电机二12工作，通过齿条二9带动移动模块二3和配重14做x轴运动。

更进一步地，所述的微型电机一11、微型电机二12和微型电机三13内设有自锁装置，防止微型电机在没有接收到重心计算控制中心1发送的驱动指令的情况下由于无人机的倾斜而导致配重异常移动，影响无人机稳定。

如图3和图4所示，实施例二中，一种搭载机械手的无人机自主平衡控制系统包括重心计算控制中心二21、移动模块四22、移动模块五23、移动模块六24、导轨槽四25、导轨槽五26、导轨槽六27、齿条四28、齿条五29、齿条六210、微型电机四211、微型电机五212、配重二214、定位挡板五215、定位挡板六216、定位挡板七217、定位挡板八218。其中，移动模块四22设置在导轨槽四25上可沿槽运动，移动模块六24设置在导轨槽六27上可沿槽运动，导轨槽五26的两端分别与移动模块四22和移动模块六24固定连接，移动模块五23设置在导轨槽五26上可沿槽运动；齿条四28设置在导轨槽四25上，齿条五29设置在导轨槽五26上，齿条六210设置在导轨槽六27上；微型电机四211设置在移动模块四22上，并且微型电机四211的转轴驱动齿条四28的动作，通过齿条四28的动作实现移动模块四22与导轨槽四25的相对限位运动，移动模块六24上未设置微型电机作为驱动导致无闭锁，移动模块六24跟随移动模块四22与导轨槽六27作相对限位运动，相对限位运动过程与移动模块四22同步；微型电机五212设置在移动模块五23上，并且微型电机五212的转轴驱动齿条五29的动作；配重二214固设在移动模块五23的下端；其中，重心计算控制中心二21、微型电机四211、微型电机五212利用无人机外接充电回路通过无人机内部电源供电，正极导线和负极导线分别布置于导轨槽四25、导轨槽五26和导轨槽六27上；定位挡板五215和定位挡板六216设置在导轨槽四25两端；定位挡板六216和定位挡板七217设置在导轨槽六27两端。所述的定位挡板五215、定位挡板六216、定位挡板七217、定位挡板八218属于限位装置，防止导轨槽五26向导轨槽四25和导轨槽六27外运动，从而限制了可动配重的运动区间。

所述的一种搭载机械手的无人机自主平衡控制系统可以通过固定连接机构安装在无人机上。

所述的重心计算控制中心二21包括整体重心位置计算分析单元和微型电机控制单元。其中，整体重心位置计算分析单元通过机械运动分析或实时状态感知掌握机械手和配重二214的重心位置，并求出无人机组合体在机械手动作过程中整体重心偏移情况，根据偏移情况求得配重二214所需移动的距离、方向以及最终应在的坐标。

所述的重心计算控制中心二21与微型电机四211、微型电机五212建立信号连接，即重心计算控制中心二21中的微型电机控制单元给微型电机四211、微型电机五212发送驱动指令，驱动微型电机四211、微型电机五212工作。微型电机四211和微型电机五212工作后，通过齿条四28带动导轨槽五26，设置在导轨槽五26上的移动模块五23、齿条五29、微型电机五212和配重二214以及与导轨槽五26固定连接的移动模块六24做y轴运动；微型电机控制单元发送给微型电机五212驱动指令后，微型电机五212工作，通过齿条五29带动移动模块五23和配重二214做x轴运动。

更进一步地，所述的微型电机四211、微型电机五212内设有自锁装置，防止微型电机在没有接收到重心计算控制中心二21发送的驱动指令的情况下由于无人机的倾斜而导致配重异常移动，影响无人机稳定。

一种搭载机械手的无人机自主平衡控制方法如下，该方法是通过调节各机翼转速完成在机械手动作时的重心平衡：

S1.系统以无人机机身本体作为基点确立水平面坐标系及坐标系原点，根据机械手每个运动单体的重心(x₁,y₁)，(x₂,y₂)，(x₃,y₃)，……，确定初始机械手组合体重心坐标(x_c1,y_c1)，确定初始无人机本体重心坐标(x_c2,y_c2)，得到机械手无人机组合体重心坐标(x_c,y_c)。

S2.当无人机机械手控制系统准备向机械手各关节下达运动指令时，根据该项运动指令计算得到各运动单体在关节执行完运动指令后的重心(x′₁,y′₁)，(x′₂,y′₂)，(x′₃,y′₃)，……，确定机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)，计算得到机械手重心偏离量(Δx₁,Δy₁)，进而得到机械手无人机组合体重心偏离量(Δx,Δy)。

S3.根据机械手无人机组合体重心偏离量(Δx,Δy)计算得到无人机机翼调整量，根据机翼数量决定每个机翼的调整分配量。

S4.在无人机机械手控制系统向机械手各关节下达运动指令时，无人机本体控制系统中的重心计算控制单元同步向无人机机翼电机下达调整分配量指令，完成在机械手操作过程中的重心稳定和平衡。

其中，如图5所示，S1中所述的初始机械手组合体重心坐标(x_c1,y_c1)的计算公式具体可以为：

式中，m_i为机械手各单体的质量，(x_i,y_i)为其相应的重心坐标值。

其中，S1中所述的机械手无人机组合体重心坐标(x_c,y_c)的计算公式具体可以为：

式中，m′为机械手总的质量，m″为无人机本体的质量。

其中，S2中所述的机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)的计算公式具体可以为：

其中，S2中所述的机械手重心偏离量(Δx₁,Δy₁)的计算公式具体可以为：

Δx₁＝x′_c1-x_c1,Δy₁＝y′_c1-y_c1。

其中，S2中所述的机械手无人机组合体重心偏离量(Δx,Δy)的计算公式具体可以为：

其中，每个机翼的调整分配量的计算公式具体可以为：F＝b·ω²，当无人机为四旋翼无人机时，式中，b为四旋翼无人机的升力参数，ω为旋翼的转速。当无人机重心靠近某个旋翼时，则该旋翼增大转速，提升该旋翼升力增大。

一种搭载机械手的无人机自主平衡控制方法如下，该方法是通过调节装载在机身上的配重完成在机械手动作时的重心平衡：

S5.首先确定机械手的重心位置(x_c1,y_c1)，配重的重心位置为(x_c2,y_c2)。

S6.无人机启动后，初始处于垂直起飞状态，重心计算控制中心1中的重心位置计算分析单元开始计算机械手以及配重的重心位置，并以二维坐标(x_c,y_c)显示。该二维坐标以无人机重心位置为原点，水平向右为x坐标，水平向上为y坐标。

S7.如果整个组合体的重心位置在无人机重心处，无人机重心位置x坐标和y坐标设为(0,0)，即x_c＝0,y_c＝0，则重心计算控制中心中的微型电机控制芯片不会给微型电机一、微型电机二和微型电机三发送驱动指令，微型电机一、微型电机二、微型电机三自锁，无人机继续执行飞行程序。

S8.若起飞时组合体的重心位置不在无人机重心位置(0,0)处，即x_c≠0或y_c≠0，则重心计算控制中心中的微型电机控制芯片会给微型电机一、微型电机二和微型电机三发送驱动指令。

S9.当机械手开始工作时，机械手会进行伸缩或转动，机械手的重心位置会发生变化。机械手的工作路径可以分为随机路径和固定路径。该无人机自主平衡控制系统根据这两种情况执行不同的驱动指令。

其中，如图5所示，S5所述的(x_c1,y_c1)计算方法如下：设机械手每个运动单体的重心(x₁,y₁)，(x₂,y₂)，(x₃,y₃)，……，则：

式中，m_i为机械手各单体的质量。因配重为规则几何体，则(x_c2,y_c2)即为配重的几何中心。

其中，S6所述的(x_c,y_c)的计算方法为：

式中，m′为机械手总的质量，m″为配重的质量。

其中，S8所述的驱动指令为：重心计算控制中心中的重心位置计算分析单元计算出机械手的重心位置的x坐标和y坐标，设为(a₁,b₁)，并将其发送给微型电机控制芯片。微型电机控制芯片驱动微型电机一和微型电机三运动到-b₁处，驱动微型电机二运动到-a₁处。微型电机一、微型电机二和微型电机三移动完成后，配重移动到(-a₁,-b₁)处，此时组合体重心位置在(0,0)处，无人机可稳定飞行。无人机稳定飞行后，微型电机一、微型电机二和微型电机三自锁。若重心计算控制中心在实时计算过程中监测到重心位置变化，则重复以上驱动过程，驱动过程如图6所示。

其中，S9所述的工作路径随机时，驱动指令如S8所述的驱动指令。

其中，S9所述的工作路径固定时，驱动指令如下：机械手的工作路径固定，即机械手的伸缩或移动路径已知，即机械手的重心移动路径已知。设机械手的重心位置x坐标和y坐标为(x′,y′)，并且x′和y′为时间t的函数，即x′＝f₂(t)，y′＝f₁(t)，设配重的重心位置x坐标和y坐标为(x″,y″)，并且x″和y″为时间t的函数根据机械手的重心位置x′坐标和y′坐标与配重的x″坐标和y″坐标关于原点对称可得x″＝-f₂(-t)，y″＝-f₁(-t)，即配重重心移动路径与机械手重心移动路径恰好相反。当机械手的重心位置沿x′＝f₂(t)，y′＝f₁(t)移动时，微型电机控制芯片按x″＝-f₂(-t)，y″＝-f₁(-t)驱动微型电机一和微型电机三运动到y″＝-f₁(-t)处，驱动微型电机二运动到x″＝-f₂(-t)处，驱动过程如图7所示。

微型电机一、微型电机二和微型电机三移动过程中，可实时保持组合体重心稳定，无人机可稳定飞行。

上述两种方法由于在重力z轴方向不会影响无人机飞行的平衡，所以只对水平面上的重心偏移量进行了计算和平衡，但z轴上的变化量可能导致无人机的上下起伏，更进一步地，将上述二维坐标转换成三维坐标通过各机翼转速控制平衡z轴的偏移量；更进一步地，将配重添加z轴方向的机械滑槽，增加z轴方向的自由度，通过配重在z轴方向的移动平衡z轴的偏移量。

本发明可实现无人机机械手组合体重心的动态调整，以防机械手工作时导致无人机失稳。在正常使用时，它可以实现无人机重心的主动平衡，相较于目前在已造成机身倾侧后才能完成无人机平衡的被动技术，本发明由于其实时的对应重心调节功能，可降低飞控系统平衡机身的负担，增强无人机的飞控性能。若已知机械手的工作路径，则可计算出配重的移动路径，可以实时保持无人机稳定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (7)

1.一种搭载机械手的无人机自主平衡控制方法，其特征在于包括如下步骤：

S1.系统以无人机机身本体作为基点确立水平面坐标系及坐标系原点，根据机械手每个运动单体的重心(x₁,y₁)，(x₂,y₂)，(x₃,y₃)，……，确定初始机械手组合体重心坐标(x_c1,y_c1)，确定初始无人机本体重心坐标(x_c2,y_c2)，得到机械手无人机组合体重心坐标(x_c,y_c)；

S2.当无人机机械手控制系统准备向机械手各关节下达运动指令时，根据该项运动指令计算得到各运动单体在关节执行完运动指令后的重心(x′₁,y′₁)，(x′₂,y′₂)，(x′₃,y′₃)，……，确定机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)，计算得到机械手重心偏离量(Δx₁,Δy₁)，进而得到机械手无人机组合体重心偏离量(Δx,Δy)；

S3.根据机械手无人机组合体重心偏离量(Δx,Δy)计算得到无人机机翼调整量，根据机翼数量决定每个机翼的调整分配量；

S4.在无人机机械手控制系统向机械手各关节下达运动指令时，无人机本体控制系统中的重心计算控制单元同步向无人机机翼电机下达机翼分配量的指令，当无人机重心靠近某个旋翼时，则该旋翼增大转速，提升该旋翼升力增大，完成在机械手操作过程中的重心稳定和平衡。

2.根据权利要求1所述的无人机自主平衡控制方法，其特征在于S1中所述的初始机械手组合体重心坐标(x_c1,y_c1)的计算公式如下：

式中，m_i为机械手各单体的质量，(x_i,y_i)为其相应的重心坐标值。

3.根据权利要求2所述的无人机自主平衡控制方法，其特征在于S1中所述的机械手无人机组合体重心坐标(x_c,y_c)的计算公式如下：

式中，m′为机械手总的质量，m″为无人机本体的质量。

4.根据权利要求1所述的无人机自主平衡控制方法，其特征在于S2中所述的机械手组合体在关节执行完运动指令后的重心坐标(x′_c1,y′_c1)的计算公式如下：

5.根据权利要求4所述的无人机自主平衡控制方法，其特征在于S2中所述的机械手重心偏离量(Δx₁,Δy₁)的计算公式如下：

Δx₁＝x′_c1-x_c1,Δy₁＝y′_c1-y_c1。

6.根据权利要求5所述的无人机自主平衡控制方法，其特征在于S2中所述的机械手无人机组合体重心偏离量(Δx,Δy)的计算公式如下：

7.根据权利要求5所述的无人机自主平衡控制方法，其特征在于S4中所述的机翼分配量的计算公式如下：

F＝b·ω²

式中，b为四旋翼无人机的升力参数，ω为旋翼的转速。

Images