本发明公开了一种飞行作业机器人冗余度机械臂下位控制器的设计方法，具体步骤包括：(1)搭建一台飞行作业机器人作为下位机；(2)将冗余度机械臂运动控制问题转换为受约束的时变凸二次规划问题；(3)将求解二次型最优解转换为求解基于线性变分不等式的原对偶神经网络；(4)将其离散化为冗余度机械臂下位控制器，写入机载微型控制器；(5)根据设计的控制器，当接收到控制指令以及轨迹参数后，下位机将求解出机械臂各关节期望角度并转化为PWM电压信号，驱动舵机打角，完成控制任务。本发明能有效解决在上位机端解算后需要传输大量数据而耗费大量时间以及出现传输错误等问题，提高了冗余度机械臂本身的实时控制效果和灵活度，具有现实意义。
1.一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，具体步骤包括：(1)搭建一台飞行作业机器人作为下位机；
(2)根据步骤(1)设计的飞行作业机器人，按照不同的设计目的和指标要求，将冗余度机械臂运动控制问题转换为受约束的时变凸二次规划问题；
(3)将求解步骤(2)中时变凸二次规划问题的二次型最优解转换为求解基于线性变分不等式的原对偶神经网络；
(4)将步骤(3)中设计的基于线性变分不等式的原对偶神经网络离散化为冗余度机械臂下位控制器并写入机载微型控制器；
(5)根据步骤(4)中设计的冗余度机械臂下位控制器，当接收到上位机传输的机械臂控制指令以及轨迹参数后，下位机将求解出机械臂各关节期望角度，并将角度信息转化为PWM电压信号，驱动舵机打角，完成控制任务。
2.根据权利要求1所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，所述步骤(1)中搭建的飞行机器人包括多旋翼飞行器和冗余度机械臂，并搭载微型控制器。
3.根据权利要求1所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，步骤(2)中逆运动学问题的逆运动学方程表示为：f(θ)＝r (1)
其中，θ表示冗余度机械臂关节角度，r表示机械臂末端的期望轨迹；f(·)表示冗余度机械臂关节角度到末端轨迹的非线性映射方程；
对逆运动学方程两边同时求导，得到冗余度机械臂在速度层上的逆运动学方程：其中， 为实数域上的m×n维矩阵；J(θ)表示冗余度机械臂的雅可比矩阵，n表示机械臂的自由度数；m表示机械臂末端轨迹的空间维数； 和 分别表示冗余度机械臂关节角度和末端轨迹关于时间的导数；
对冗余度机械臂在速度层上的逆运动学方程两边同时求导，得到冗余度机械臂在加速度层上的逆运动学方程：其中，J(θ)表示冗余度机械臂的雅可比矩阵， 表示冗余度机械臂关节加速度；
4.根据权利要求3所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，步骤(2)的逆运动学问题按照不同的设计目的和指标要求，转化为受约束的时变凸二次规划问题，表示方式为：s.t.Ax＝b (5)
其中，Ax＝b表示完成相应任务所需的等式约束；Cx≤d表示用于环境障碍物躲避的不等式约束，C与d根据相应的避障问题具有不同的参数值； 与 表示对应关节角的双端不等式约束。
5.根据权利要求4所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，针对速度层的重复运动规划方案求解时公式(4)-(7)各参数取值如下：c＝λ(θ-θ(0))，λ>0；A＝J(θ)； 与 为对应关节角 的双端不等式约束，与 为关节i的角度极限， 与 为关节i的角速度极限，k1∈R为调节关节角速度可行域的强度系数，满足k1>
6.根据权利要求4所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，针对加速度层的重复运动规划方案求解时公式(4)-(7)各参数取值如下：μ>0且v>0；
加速度极限，k2∈R为调节关节角加速度可行域的强度系数，满足k2>0。
7.根据权利要求1所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，在步骤(3)中，将求解上述二次规划问题转化为寻求一个满足下述线性变分不*等式的原对偶变量u，其表示方式为：
(u-u*)T(Mu*+q)≥0 (8)其中，系数矩阵 W＝I∈Rn
8.根据权利要求7所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，将寻求一个满足线性微分不等式的原对偶变量u*的问题转换为分段线性投影方程：PΩ(u-(Mu+q))-u＝0 (10)
其中，PΩ(·)是一个从R 到Ω的投影算子，其具体的函数关系为：根据动力学系统求解经验，设计下述基于线性变分不等式的原对偶神经网络来对公式(10)进行求解：其中，β>0用于调节所述神经网络的收敛率；根据公式(9)，公式(12)求解得到的最优解u*的前n个元素即为机械臂对应的关节角参数x。
9.根据权利要求8所述的一种飞行作业机器人冗余度机械臂下位控制器的设计方法，其特征在于，所述步骤(4)中，将上述设计的基于线性变分不等式的原对偶神经网络应用到微型控制器时，需要将其离散化为冗余度机械臂控制器；先将时间离散化为时刻，定义求解的时间区间T＝[t0,tend]，定义时间步长为tstep，因此把连续的时间区间T离散化为个时刻；
机械臂在t0时刻开始运动，在tend时刻结束运动，再采用极坐标分别表示期望轨迹r、期望轨迹关于时间的偏导数 期望轨迹关于时间的二次偏导数 最后将公式离散化为：若已知u0，通过公式(13)中迭代的方式求解得到任意(t+1)时刻的ut+1；根据公式(9)，ut+1的前n个元素即为此时机械臂对应的关节角参数xt+1；
若x表示角速度，即xt+1＝(θt+1-θt)/tstep＝Δθt+1/tstep，通过下述增量式积分得到(t+1)时刻机械臂对应关节角θt+1：θt+1＝θt+tstep·xt+1 (14)若x表示角加速度，即xt+1＝(Δθt+1-Δθt)/tstep，先通过下述增量式积分得到机械臂对应关节角的Δθt+1，再根据公式(14)求得θt+1：Δθt+1＝Δθt+tstep·xt+1 (15)。
一种飞行作业机器人冗余度机械臂下位控制器的设计方法
技术领域
[0001] 本发明涉及飞行作业机器人的设计领域，尤其涉及一种飞行作业机器人冗余度机械臂下位控制器的设计方法。
背景技术
[0002] 伴随着智能机器人领域的发展，飞行机器人受到了越来越多人的关注，特别是装载冗余度机械臂的多旋翼飞行机器人，更在智能机器人领域备受瞩目。该飞行机器人主要由多旋翼飞行器和冗余度机械臂两部分组成。由于具有冗余度机械臂，该飞行机器人具备执行复杂任务、躲避关节极限、躲避奇异点的能力，并且具有一定的容错特性，适用于解决各类高空作业，能够取代“蜘蛛人”完成各项危险任务，具有很高的研究价值。
[0003] 飞行机器人的冗余度机械臂的运动控制需要解析逆运动学问题，以往在上位机端解析后再实时传输结果到下位机端的控制方式，存在传输大量数据耗费大量时间、可能出现传输错误等问题。
发明内容
[0004] 本发明的目的在于提供一种飞行作业机器人冗余度机械臂下位控制器的设计方法。所述方法根据上位机简单、少量的轨迹参数即可进行逆运动学实时求解，有效解决在上位机端解算后需要传输大量数据耗费大量时间以及可能出现传输错误等问题，提高机械臂本身的实时控制效果和灵活度，具有现实意义。
[0005] 本发明的目的能够通过以下技术方案实现：
[0006] 一种飞行作业机器人冗余度机械臂下位控制器的设计方法，具体步骤包括：
[0007] (1)搭建一台飞行作业机器人作为下位机；
[0008] (2)根据步骤(1)设计的飞行作业机器人，按照不同的设计目的和指标要求，将冗余度机械臂运动控制问题转换为受约束的时变凸二次规划问题；
[0009] (3)将求解步骤(2)中时变凸二次规划问题的二次型最优解转换为求解基于线性变分不等式的原对偶神经网络(LVI-PDNN)；
[0010] (4)将步骤(3)中设计的LVI-PDNN离散化为冗余度机械臂下位控制器并写入机载微型控制器；
[0011] (5)根据步骤(4)中设计的冗余度机械臂下位控制器，当接收到上位机传输的机械臂控制指令以及轨迹参数后，下位机将求解出机械臂各关节期望角度，并将角度信息转化为PWM电压信号，驱动舵机打角，完成控制任务。
[0012] 具体地，搭建的飞行机器人包括多旋翼飞行器和冗余度机械臂，并搭载微型控制器。
[0013] 进一步地，上述冗余度机械臂运动控制问题的逆运动学方程表示为：
[0014] f(θ)＝r (1)
[0015] 其中，θ表示冗余度机械臂关节角度，r表示机械臂末端的期望轨迹；f(·)表示冗余度机械臂关节角度到末端轨迹的非线性映射方程。
[0016] 对逆运动学方程两边同时求导，得到冗余度机械臂在速度层上的逆运动学方程：
[0017]
[0018] 其中， 为实数域上的m×n维矩阵；J(θ)表示冗余度机械臂的雅可比矩阵，n表示机械臂的自由度数；m表示机械臂末端轨迹的空间维数； 和 分别表示冗余度机械臂关节角度和末端轨迹关于时间的导数。
[0019] 对冗余度机械臂在速度层上的逆运动学方程两边同时求导，得到冗余度机械臂在加速度层上的逆运动学方程：
[0020]
[0021] 其中，J(θ)表示冗余度机械臂的雅可比矩阵， 表示冗余度机械臂关节加速度；表示雅可比矩阵关于时间的导数。
[0022] 具体地，在所述步骤(2)中，上述逆运动学问题可以按照不同的设计目的和指标要求，转化为受约束的时变凸二次规划问题，其表示方式为：
[0023]
[0024] s.t.Ax＝b(5)
[0025] Cx≤d(6)
[0026]
[0027] 其中，Ax＝b表示完成相应任务所需的等式约束；Cx≤d表示用于环境障碍物躲避的不等式约束，C与d根据相应的避障问题具有不同的参数值； 与 表示对应关节角的双端不等式约束。
[0028] 具体地，针对不同的设计目的和指标要求，上述受约束的时变凸二次规划问题有不同的表达形式。其中，针对速度层的重复运动规划方案求解时，公式(4)-(7)各参数取值如下： c＝λ(θ-θ(0))，λ＞0；A＝J(θ)； 与 为对应关节角 的双端不等式约束， C与d根据相
应的避障问题具有不同的参数值， 与 为关节i的角度极限， 与 为关节i的角速度极限，k1∈R为调节关节角速度可行域的强度系数，满足k1＞0。
[0029] 针对加速度层的重复运动规划方案求解时，公式(4)-(7)各参数取值如下：μ＞0且v＞0；
与 为对应关节角 的双端不等式约束，
C与d根据相应的避障问题具
有不同的参数值， 与 为关节i的角加速度极限，k2∈R为调节关节角加速度可行域的强度系数，满足k2＞0。
[0030] 具体地，在步骤(3)中，将求解上述二次规划问题转化为寻求一个满足下述线性变分不等式(LVI)的原对偶变量u*，其表示方式为：
[0031] (u-u*)T(Mu*+q)≥0 (8)
[0032] 其中，系数矩阵 Wn×n * - + n+m *
＝I∈R 。u∈Ω＝{u|u＜u＜u}∈R ，对u的不等式约束分别定义：
[0033]
[0034] 其中，y是对偶决策变量，l＝[1…1]T是一个全为1的列向量，ω＞＞1∈R是用来近似代表无穷大的正数，如可取ω＝1×1010。
[0035] 更进一步地，能够将寻求一个满足下述LVI的原对偶变量u*的问题转换为分段线性投影方程(PLPEs)：
[0036] PΩ(u-(Mu+q))-u＝0(10)
[0037] 其中，PΩ(·)是一个从Rn+m到Ω的投影算子，其具体的函数关系为：
[0038]
[0039] 根据动力学系统求解经验，设计下述LVI-PDNN来对公式(10)进行求解：
[0040]
[0041] 其中，β＞0用于调节神经网络的收敛率。根据公式(9)，公式(12)求解得到的最优解u*的前n个元素即为机械臂对应的关节角参数x。
[0042] 进一步地，将上述设计的LVI-PDNN应用到微型控制器时，需要将其离散化为冗余度机械臂控制器。先将时间离散化为时刻，定义求解的时间区间T＝[t0,tend]，定义时间步长为tstep，因此把连续的时间区间T离散化为 个时刻。
[0043] 机械臂在t0时刻开始运动，在tend时刻结束运动，再采用极坐标分别表示期望轨迹r、期望轨迹关于时间的偏导数 期望轨迹关于时间的二次偏导数 最后将公式离散化为：
[0044]
[0045] 若已知u0，即可通过公式(13)中迭代的方式求解得到任意(t+1)时刻的ut+1。根据公式(9)，ut+1的前n个元素即为此时机械臂对应的关节角参数xt+2。
[0046] 若x表示角速度，即xt+1＝(θt+1-θt)/tstep＝Δθt+1/tstep，通过下述增量式积分即可得到(t+1)时刻机械臂对应关节角θt+1：
[0047] θt+1＝θt+tstep·xt+1(14)
[0048] 若x表示角加速度，即xt+1＝(Δθt+1-Δθt)/tstep，可先通过下述增量式积分得到机械臂对应关节角的Δθt+1，再根据公式(14)求得θt+1：
[0049] Δθt+1＝Δθt+tstep·xt+1(15)
[0050] 进一步地，将上述离散化的冗余度机械臂控制器转换成微型控制器代码，将代码写入下位机端机载微型控制器并在微型控制器上运行。当下位机端接收到上位机传输的冗余度机械臂控制指令以及相应的轨迹参数后，下位机端机载微型控制器可根据该冗余度机械臂控制器求解出机械臂各关节期望角度，并将角度信息转化为PWM电压信号，驱动舵机打角，完成控制任务。
[0051] 本发明相较于现有技术，具有以下的有益效果：
[0052] 本发明的飞行作业机器人冗余度机械臂下位控制器的控制程序运行在机载微型控制器上，根据上位机简单、少量的轨迹参数即可进行逆运动学实时求解，有效解决在上位机端解算后需要传输大量数据而耗费大量时间、可能出现传输错误等问题，提高冗余度机械臂本身的实时控制效果和灵活度。
附图说明
[0053] 图1为本发明中一种飞行作业机器人机械臂下位控制器的设计流程图；
[0054] 图2为本发明中一种飞行作业机器人的系统框图；
[0055] 图3为本发明所设计的一种飞行作业机器人机械臂下位控制器的具体执行流程图。
[0056] 图中，1—PC端上位机、2—多旋翼飞行器、3—冗余度机械臂、4—控制电路。
具体实施方式
[0057] 下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0058] 实施例
[0059] 如图1所示为本发明的具体设计流程图。一种飞行作业机器人冗余度机械臂下位控制器的设计方法，具体步骤包括：
[0060] (1)搭建一台飞行作业机器人作为下位机；
[0061] 如图2所示为本发明的系统框图。所述系统由PC端上位机1和飞行作业机器人下位机组成。
[0062] 具体地，飞行作业机器人下位机主要由多旋翼飞行器2和冗余度机械臂3组成。
[0063] 下位机端搭载的控制电路4中包括微型控制器、无线通信模块以及驱动电路等，并在机载微型控制器运行根据本发明设计的冗余度机械臂运动规划控制器。
[0064] (2)根据步骤(1)设计的飞行作业机器人，按照不同的设计目的和指标要求，将冗余度机械臂运动控制问题转换为受约束的时变凸二次规划问题；
[0065] 在本实施例中，设计机械臂控制器时，按照速度层上进行重复运动的设计目的，提出如下指标：
[0066]
[0067] 其中，c＝λ(θ-θ(0))，λ∈R表示用来控制关节位移幅值的正设计参数。对上述指标引入物理约束，包括关节角度上下限和角速度上下限，具体表示为：
[0068]
[0069]
[0070] θ-≤θ≤θ+
[0071]
[0072] 将上述机械臂运动控制问题转换为受约束的时变凸二次规划问题，具体表示为：
[0073]
[0074]
[0075]
[0076] (3)将求解步骤(2)中时变凸二次规划问题的二次型最优解转换为求解基于线性变分不等式的原对偶神经网络(LVI-PDNN)；
[0077] 根据公式(8)-(11)，设计线性变分不等式的原对偶神经网络(LVI-PDNN)来求解步骤(2)中的时变凸二次规划问题，设计的线性变分不等式的原对偶神经网络具体表示形式为：
[0078]
[0079] 其中， 表示原对偶决策变量。
[0080] (4)将步骤(3)中设计的LVI-PDNN离散化为冗余度机械臂下位控制器并写入机载微型控制器；
[0081] 将步骤(3)中设计的LVI-PDNN离散化为冗余度机械臂控制器，具体为：
[0082]
[0083] 假设机械臂末端期望轨迹为一个半径为a的平面圆，以机械臂初始状态时末端执行器的位置为原点，用极坐标对该平面圆进行表示，表示方式为：
[0084] r2-2arcosφ＝0(20)
[0085] 其中，φ∈[0,2π]。
[0086] 将φ离散化，对应连续的时间区间T离散化成 个时刻。
[0087] 将公式(12)转化为空间直角坐标系的形式，即
[0088]
[0089] (5)根据步骤(4)中设计的冗余度机械臂下位控制器，当接收到上位机传输的机械臂控制指令以及轨迹参数后，下位机将求解出机械臂各关节期望角度，并将角度信息转化为PWM电压信号，驱动舵机打角，完成控制任务。
[0090] 将离散化的机械臂控制器即公式(13)和本实施例中的轨迹方程即公式(20)(21)写入机载微型控制器。
[0091] 如图3所示为本发明的具体执行流程图。当进行机械臂运动规划控制时，PC端上位机传输简单、少量的机械臂控制指令和相应的轨迹参数，如绘制半径为30cm的平面圆，到下位机端，下位机端机载微型控制器解析该机械臂控制指令和轨迹参数，并运行冗余度机械臂运动规划控制器程序，判断机械臂运动控制任务进度，通过递推的方式求解出机械臂各关节期望角度，并将求解得到的角度信息转化为PWM电压信号，驱动机械臂舵机打角。之后再次判断机械臂运动规划控制任务进度，如果还未完成整个运动规划控制任务，则重复上述求解角度、转换信号、驱动舵机的步骤，直至完成整个运动规划控制任务，然后结束工作。
[0092] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
