CN106064377A - 一种空间机器人动力学参数辨识的激励轨迹优化方法 - Google Patents

Abstract

本发明公开了一种空间机器人动力学参数辨识的激励轨迹优化方法，可以实现空间机器人动力学参数辨识的激励轨迹优化，同时满足参数辨识的PE条件和机械臂关节运动约束，提高空间机器人参数辨识的收敛速度和准确性。自由漂浮空间机器人的非完整特性决定了参数辨识模型回归矩阵A(k)不仅含有与机械臂关节运动轨迹相关的关节位置和关节速度，还含有与待辨识动力学参数间接相关的基座位置、姿态、速度和角速度，而这些量必须根据系统的动力学模型进行求解，因此在激励轨迹离线优化时需要用到待辨识动力学参数的先验信息。

Description

一种空间机器人动力学参数辨识的激励轨迹优化方法

【技术领域】

本发明属于空间机器人领域，涉及一种空间机器人动力学参数辨识的激励轨迹优化方法。

【背景技术】

空间机器人完成在轨服务任务时一般采用基于模型的控制方法，这类方法的控制精度与空间机器人动力学参数的准确程度紧密相关。在进行空间机器人设计和加工时能够通过CAD软件计算其动力学参数，也能够通过地面测试得到其各个部件精确的动力学参数。然而空间机器人在轨捕获目标后整个系统的动力学参数会发生相应改变，为了保证后续操作中控制系统的稳定性并提高控制精度，需要对捕获目标后的空间机器人动力学参数进行辨识并利用辨识得到的动力学参数更新控制器的相关参数。

动力学参数辨识的准确性和快速性不仅和选用的参数辨识模型以及估计方法有关，也与参数辨识的激励轨迹有很大关系。为了加快参数辨识的收敛速度并提高参数估计的准确性，需要合理选择空间机器人参数辨识的激励轨迹，以保证用于辨识的测量数据满足持续激励(PE)条件。

地面工业机器人一般是通过离线设计激励轨迹来满足动力学参数辨识所需的PE条件，但自由漂浮空间机器人由于其非完整约束特性，导致动力学参数辨识模型的回归矩阵中含有和待辨识动力学参数相关的状态量(基座的位置、姿态、速度和角速度)，这些状态量与关节运动轨迹相关，无法预先单独设计，需要根据系统的动力学模型进行解算。因此不同于地面工业机器人，空间机器人的激励轨迹优化需要用到待辨识参数的先验信息。

虽然一些空间机器人动力学参数辨识方面的文献提到了激励轨迹需要满足PE条件，但多数文献仅是在完成参数辨识后验证参数辨识模型回归矩阵的条件数是否足够小，目前已发表的文献中只有很少几篇涉及空间机器人动力学参数辨识的激励轨迹优化问题。文献“Modeling and experimental design for the on-orbit inertial parameteridentification of free-flying space robots”中采用机械臂运动对空间机器人基座的扰动幅度来衡量激励轨迹的激励程度并根据该指标优化激励轨迹，然而该指标的提出更多是出于一种直观感觉，缺乏准确的理论支撑。文献“Parameter identification methodsfor free-floating space robots with direct torque sensing”采用B样条参数化表示空间机器人的机械臂运动轨迹并根据设计的优化准则优化激励轨迹，然而其轨迹优化的目的是为了更好地激励空间机器人的柔性模态来研究空间机器人的柔性附件和燃料晃动对参数辨识的影响，而并非通过优化使激励轨迹满足参数辨识所需的PE条件。

【发明内容】

本发明的目的在于针对自由漂浮空间机器人动力学参数辨识问题，提供一种空间机器人动力学参数辨识的激励轨迹优化方法，用于在机械臂关节运动约束范围内提高空间机器人动力学参数辨识的收敛速度和准确性，保证用于参数辨识的测量信息满足持续激励(PE)条件且不违背机械臂的关节运动约束。

为达到上述目的，本发明采用以下技术方案予以实现：

一种空间机器人动力学参数辨识的激励轨迹优化方法，包括以下步骤：

1)建立自由漂浮空间机器人的参数辨识模型

空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由n个旋转关节连接而成，所有构件均视为刚体；基座航天器上安装有测量基座位置、姿态、线速度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器；

空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_s\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_m\stackrel{\·}{\mathrm{\φ}}---\left(1\right)$

式中：

$J_s=\left[\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right]\∈R^{6\×6},p_{0e}=p_e-r_0---\left(2\right)$

$J_m=\left[\begin{array}{cccc}{k}_1\×(p_e-p_1)& k_2\×(p_e-p_2)& ...& k_n\×(p_e-p_n)\\ k_1& k_2& ...& k_n\end{array}\right]\∈R^{6\×n}---\left(3\right)$

空间机器人的线动量P和角动量L用矩阵形式统一表示为：

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\\ O& H_{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+\left[\begin{array}{c}{J}_{T\mathrm{\ω}}\\ H_{\mathrm{\ω}\mathrm{\φ}}\end{array}\right]\stackrel{\·}{\mathrm{\φ}}+\left[\begin{array}{c}O\\ r_g\×P\end{array}\right]---\left(4\right)$

式中：

$H_{\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}(I_i+m_i{\tilde{r}}_{gi}^T{\tilde{r}}_{0i})+I_0\∈R^{3\×3}---\left(5\right)$

$H_{\mathrm{\ω}\mathrm{\φ}}=\underset{k=1}{\overset{n}{\Σ}}(I_i{J}_{Ri}+m_i{\tilde{r}}_{gi}{J}_{Ti})\∈R^{3\×n}---\left(6\right)$

$J_{T\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}{m}_i{J}_{Ti}---\left(7\right)$

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),…,k_i×(r_i-p_i),0,…,0] (8)

J_Ri＝[k₁,k₂,…,k_i,0,…,0] (9)

r_0g＝r_g-r₀,r_gi＝r_i-r_g,r_0i＝r_i-r₀ (10)

设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理得到：

式中：

i_n＝[I_xx,-I_xy,-I_xz,I_yy,-I_yz,I_zz]^T

${\mathrm{\Ω}}_n=R_n^I\left[\begin{array}{cccccc}{\mathrm{\ω}_n}_{nx}& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0& 0& 0\\ 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0\\ 0& 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}\end{array}\right]$

$u={\mathrm{\Σ}}_{i=0}^{n-1}{m}_i{\stackrel{\·}{r}}_i$

$q={\mathrm{\Σ}}_{i=0}^{n-1}(I_i{\mathrm{\ω}}_i+r_i\×m_i{\stackrel{\·}{r}}_i)+u\×p_n$

构造一个以x＝[1/m_n ⁿa_n ^T i_n ^T]^T为未知量的线性回归方程组进行求解；假设在每个采样点获取一组测量值，当完成第k次采样后，线性回归方程组能够表示如下：

A(k)x＝Y(k) (12)

式中：

$A\left(k\right)=\left[\begin{array}{c}{A}_1\\ A_2\\ .\\ .\\ .\\ A_k\end{array}\right],Y\left(k\right)=\left[\begin{array}{c}{y}_1\\ y_2\\ .\\ .\\ .\\ y_k\end{array}\right]---\left(13\right)$

$A_i={\left[\begin{array}{ccc}u& {\widetilde{\mathrm{\ω}}}_0{R}_n^I+\underset{j=1}{\overset{n}{\Σ}}{\tilde{k}}_j{\stackrel{\·}{\mathrm{\φ}}}_j{R}_n^I& O\\ O& \tilde{u}{R}_n^I& {\mathrm{\Ω}}_n\end{array}\right]}_i,i=1,2,...,k---\left(14\right)$

$y_i=-{\left[\begin{array}{c}{v}_0+{\mathrm{\ω}}_0\×(p_n-r_0)+\underset{j=1}{\overset{n}{\Σ}}(k_j\×(p_n-p_j\left)\right){\stackrel{\·}{\mathrm{\φ}}}_j\\ q\end{array}\right]}_i,i=1,2,...,k---\left(15\right)$

待辨识的动力学参数x可采用最小二乘算法进行估计：

$\hat{x}={\left(A^T\right(k\left)A\right(k\left)\right)}^{-1}{A}^T\left(k\right)Y\left(k\right)---\left(16\right)$

2)确定激励轨迹优化准则

选用回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：

$J={\mathrm{cond}}_2\left(A\right(k\left)\right)=\frac{{\mathrm{\σ}}_{max}\left(A\right(k\left)\right)}{{\mathrm{\σ}}_{\min }\left(A\right(k\left)\right)}---\left(17\right)$

式中：σ_max(A(k))表示矩阵A(k)的最大奇异值，σ_min(A(k))表示矩阵A(k)的最小奇异值；

3)空间机器人关节轨迹参数化

空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：

${\mathrm{\φ}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(\frac{a_l^i}{{\mathrm{\ω}}_{f}l}\sin \right({\mathrm{\ω}}_{f}lt)-\frac{b_l^i}{{\mathrm{\ω}}_{f}l}\cos ({\mathrm{\ω}}_{f}lt\left)\right)+{\mathrm{\φ}}_{i0}---\left(18\right)$

式中：ω_f为傅立叶级数的基频，φ_i0为关节位置偏移量；机械臂各关节采用相同的基频来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2N+1个待定系数；

对式(18)关于时间求一阶导数和二阶导数能够得到关节i的角速度和角加速度如下所示：

${\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(a_l^i\cos \right({\mathrm{\ω}}_{f}lt)+b_l^i\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(19\right)$

${\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)={\mathrm{\ω}}_f\underset{l=1}{\overset{N}{\Σ}}\left(b_l^{i}l\cos \right({\mathrm{\ω}}_{f}lt)-a_l^{i}l\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(20\right)$

4)激励轨迹优化问题求解

激励轨迹优化问题描述为如下形式：

φ^*(t)＝argmin(J) (21)

约束条件为：

$\left\{\begin{array}{cc}{\mathrm{\φ}}_i\left(t_0\right)={\mathrm{\φ}}_{i0},{\mathrm{\φ}}_i\left(t_f\right)={\mathrm{\φ}}_{if}& \∀i\\ {\stackrel{\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\mathrm{\φ}}_i^{\min }\≤{\mathrm{\φ}}_i\left(t\right)\≤{\mathrm{\φ}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\end{array}\right.---\left(22\right)$

式(21)和式(22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的fmincon函数求得该非线性优化问题的解，即步骤3)中待定系数和φ_i0的值。

与现有技术相比，本发明具有以下有益效果：

本发明可以实现空间机器人动力学参数辨识的激励轨迹优化，同时满足参数辨识的PE条件和机械臂关节运动约束，提高空间机器人参数辨识的收敛速度和准确性。自由漂浮空间机器人的非完整特性决定了参数辨识模型回归矩阵A(k)不仅含有与机械臂关节运动轨迹相关的关节位置和关节速度，还含有与待辨识动力学参数间接相关的基座位置、姿态、速度和角速度，而这些量必须根据系统的动力学模型进行求解，因此在激励轨迹离线优化时需要用到待辨识动力学参数的先验信息。但仿真结果表明该方法对先验误差具有较强的鲁棒性，在先验值和真实值具有50％误差的情况下，通过优化方法得到的激励轨迹仍可以满足PE条件，因此该方法具有很好的工程适用性。

【附图说明】

图1为空间机器人模型图；

图2为最优激励轨迹的关节角度变化曲线图；

图3为最优激励轨迹的关节角速度变化曲线图；

图4为对照组激励轨迹的关节角度变化曲线图；

图5为对照组激励轨迹的关节角速度变化曲线图；

图6为质量辨识结果图；

图7为转动惯量辨识结果图；

图8为质心位置辨识结果图；

图9为回归矩阵条件数图；

图10为捕获目标后的空间机器人系统图；

图11为激励轨迹优化求解流程图。

其中：1-连杆1；2-连杆2；3-连杆3；4-连杆4；5-连杆5；6-连杆6；7-连杆7；8-基座。

【具体实施方式】

下面结合附图对本发明做进一步详细描述：

参见图1-图11，本发明空间机器人动力学参数辨识的激励轨迹优化方法，其特征在于，包括以下步骤：

步骤1：建立自由漂浮空间机器人的参数辨识模型

空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由n个旋转关节连接而成，所有构件均视为刚体。基座航天器上安装有测量基座位置、姿态、线速度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器。

空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_s\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_m\stackrel{\·}{\mathrm{\φ}}---\left(1\right)$

式中：

$J_s=\left[\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right]\∈R^{6\×6},p_{0e}=p_e-r_0---\left(2\right)$

$J_m=\left[\begin{array}{cccc}{k}_1\×(p_e-p_1)& k_2\×(p_e-p_2)& ...& k_n\×(p_e-p_n)\\ k_1& k_2& ...& k_n\end{array}\right]\∈R^{6\×n}---\left(3\right)$

空间机器人的线动量P和角动量L用矩阵形式统一表示为：

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\\ O& H_{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+\left[\begin{array}{c}{J}_{T\mathrm{\ω}}\\ H_{\mathrm{\ω}\mathrm{\φ}}\end{array}\right]\stackrel{\·}{\mathrm{\φ}}+\left[\begin{array}{c}O\\ r_g\×P\end{array}\right]---\left(4\right)$

式中：

$H_{\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}(I_i+m_i{\tilde{r}}_{gi}^T{\tilde{r}}_{0i})+I_0\∈R^{3\×3}---\left(5\right)$

$H_{\mathrm{\ω}\mathrm{\φ}}=\underset{k=1}{\overset{n}{\Σ}}(I_i{J}_{Ri}+m_i{\tilde{r}}_{gi}{J}_{Ti})\∈R^{3\×n}---\left(6\right)$

$J_{T\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}{m}_i{J}_{Ti}---\left(7\right)$

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),…,k_i×(r_i-p_i),0,…,0] (8)

J_Ri＝[k₁,k₂,…,k_i,0,…,0] (9)

r_0g＝r_g-r₀,r_gi＝r_i-r_g,r_0i＝r_i-r₀ (10)

设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理可得：

式中：

i_n＝[I_xx,-I_xy,-I_xz,I_yy,-I_yz,I_zz]^T

${\mathrm{\Ω}}_n=R_n^I\left[\begin{array}{cccccc}{\mathrm{\ω}_n}_{nx}& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0& 0& 0\\ 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0\\ 0& 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}\end{array}\right]$

$u={\mathrm{\Σ}}_{i=0}^{n-1}{m}_i{\stackrel{\·}{r}}_i$

$q={\mathrm{\Σ}}_{i=0}^{n-1}(I_i{\mathrm{\ω}}_i+r_i\×m_i{\stackrel{\·}{r}}_i)+u\×p_n$

(11)是一个含有10个未知数，6个独立方程的线性方程组，因此仅靠一组测量数据无法得到确定的解，需要利用多组测量数据，构造一个以x＝[1/m_n ⁿa_n ^T i_n ^T]^T为未知量的线性回归方程组进行求解。假设在每个采样点获取一组测量值，当完成第k次采样后，线性回归方程组可以表示如下：

A(k)x＝Y(k) (12)

式中：

$A\left(k\right)=\left[\begin{array}{c}{A}_1\\ A_2\\ .\\ .\\ .\\ A_k\end{array}\right],Y\left(k\right)=\left[\begin{array}{c}{y}_1\\ y_2\\ .\\ .\\ .\\ y_k\end{array}\right]---\left(13\right)$

$A_i={\left[\begin{array}{ccc}u& {\widetilde{\mathrm{\ω}}}_0{R}_n^I+\underset{j=1}{\overset{n}{\Σ}}{\tilde{k}}_j{\stackrel{\·}{\mathrm{\φ}}}_j{R}_n^I& O\\ O& \tilde{u}{R}_n^I& {\mathrm{\Ω}}_n\end{array}\right]}_i,i=1,2,...,k---\left(14\right)$

$y_i=-{\left[\begin{array}{c}{v}_0+{\mathrm{\ω}}_0\×(p_n-r_0)+\underset{j=1}{\overset{n}{\Σ}}(k_j\×(p_n-p_j\left)\right){\stackrel{\·}{\mathrm{\φ}}}_j\\ q\end{array}\right]}_i,i=1,2,...,k---\left(15\right)$

待辨识的动力学参数x可采用最小二乘算法进行估计：

$\hat{x}={\left(A^T\right(k\left)A\right(k\left)\right)}^{-1}{A}^T\left(k\right)Y\left(k\right)---\left(16\right)$

步骤2:确定激励轨迹优化准则

线性回归方程组A(k)x＝Y(k)中系数矩阵A(k)的条件数可以度量方程组的解x对于误差的敏感性。如果A(k)的条件数较大，Y(k)的微小改变就会引起x较大的改变，从而降低数值稳定性。

在参数辨识中，经常采用辨识模型的回归矩阵条件数来衡量参数辨识的激励水平，为了保证参数辨识的准确性和收敛速度，需要尽量减小回归矩阵的条件数。本发明选用回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：

$J={\mathrm{cond}}_2\left(A\right(k\left)\right)=\frac{{\mathrm{\σ}}_{max}\left(A\right(k\left)\right)}{{\mathrm{\σ}}_{\min }\left(A\right(k\left)\right)}---\left(17\right)$

式中：σ_max(A(k))表示矩阵A(k)的最大奇异值，σ_min(A(k))表示矩阵A(k)的最小奇异值。

步骤3：空间机器人关节轨迹参数化

空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：

${\mathrm{\φ}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(\frac{a_l^i}{{\mathrm{\ω}}_{f}l}\sin \right({\mathrm{\ω}}_{f}lt)-\frac{b_l^i}{{\mathrm{\ω}}_{f}l}\cos ({\mathrm{\ω}}_{f}lt\left)\right)+{\mathrm{\φ}}_{i0}---\left(18\right)$

式中：ω_f为傅立叶级数的基频，φ_i0为关节位置偏移量。机械臂各关节采用相同的基频来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2N+1个待定系数，根据步骤4中的方法确定和φ_i0的值能够保证空间机器人的运动满足参数辨识的PE条件。

对式(18)关于时间求一阶导数和二阶导数可以得到关节i的角速度和角加速度如下所示：

${\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(a_l^i\cos \right({\mathrm{\ω}}_{f}lt)+b_l^i\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(19\right)$

${\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)={\mathrm{\ω}}_f\underset{l=1}{\overset{N}{\Σ}}\left(b_l^{i}l\cos \right({\mathrm{\ω}}_{f}lt)-a_l^{i}l\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(20\right)$

步骤4:激励轨迹优化问题求解

空间机器人机械臂的关节运动不仅要满足参数辨识的激励要求，还需要满足关节运动范围、关节角速度、关节角加速度的约束。因此激励轨迹优化问题可以描述为如下形式：

φ^*(t)＝argmin(J) (21)

约束条件为：

$\left\{\begin{array}{cc}{\mathrm{\φ}}_i\left(t_0\right)={\mathrm{\φ}}_{i0},{\mathrm{\φ}}_i\left(t_f\right)={\mathrm{\φ}}_{if}& \∀i\\ {\stackrel{\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\mathrm{\φ}}_i^{\min }\≤{\mathrm{\φ}}_i\left(t\right)\≤{\mathrm{\φ}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\end{array}\right.---\left(22\right)$

式(21)和式(22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的fmincon函数可求得该非线性优化问题的解，即步骤3中待定系数和φ_i0的值。

本发明的原理：

针对图1所示的空间机器人模型，采用本发明得到的最优激励轨迹如图2和图3所示。空间机器人(含目标)的动力学参数如表1所示，7自由度机械臂的Denavit-Hatenberg(DH)参数如表2所示，关节运动约束如表3所示。

表1空间机器人动力学参数

表2机械臂DH参数

表3机械臂关节运动约束

图2和图3所示的最优激励轨迹根据动力学参数先验值计算的回归矩阵条件数为1.6912。为了说明本发明的效果，选取图4和图5所示的对照组激励轨迹，该激励轨迹由频率为0.1Hz和0.5Hz的两种正弦谐波组成，满足机械臂关节角度和关节角速度的初始及终端约束条件，对照组激励轨迹根据参数先验值计算的回归矩阵条件数为510.6878。

控制空间机器人分别跟踪最优激励轨迹和对照组激励轨迹，根据两组采样数据分别完成对空间机器人动力学参数的辨识，辨识结果如图6至图8所示。从仿真结果可以看出，最优激励轨迹对应的参数辨识结果收敛速度更快、准确性更高。虽然仿真中的最优激励轨迹是根据待辨识参数的先验值求得的，但从图9所示的回归矩阵条件数仿真结果可以看出，本发明的激励轨迹设计方法对于先验误差具有较强的鲁棒性，在参数先验值仅为真实值一半的情况下，设计的激励轨迹仍具有较小的回归矩阵条件数，能够满足PE条件，从而保证了参数辨识的快速性和准确性。

图10是捕获目标后空间机器人系统的结构模型，其中：

Σ_I:惯性坐标系，原点为O_I；

Σ_B:空间机器人基座的体坐标系，原点O_b位于基座质心；

m₀,m_i:分别表示基座和连杆i的质量，系统总质量

I₀,I_i∈R^3×3:分别表示基座和连杆i绕各自质心的惯性张量；

k_i∈R³:表示关节i旋转方向的单位向量；

r_i∈R³:连杆i质心的位置向量；

r_g∈R³:空间机器人系统质心O_g的位置向量；

r₀∈R³:空间机器人基座质心O_b的位置向量；

p_i∈R³:关节i的位置向量；

p_e∈R³:机械臂末端执行器的位置向量；

b₀∈R³:从基座质心O_b指向关节1的位置矢量；

a_i,b_i∈R³:分别为从关节i指向连杆i质心，从连杆i质心指向关节i+1的位置矢量；

v_i∈R³:连杆i质心的线速度；

ω_i∈R³:连杆i的角速度；

：表示从坐标系Σ_i到坐标系Σ_j的旋转矩阵；

v₀∈R³:基座的线速度；

ω₀∈R³:基座的角速度；

v_e∈R³:机械臂末端执行器的线速度；

ω_e∈R³:机械臂末端执行器的角速度；

φ∈Rⁿ:机械臂关节角向量；

E,O:分别为单位矩阵和零矩阵；

此外，定义向量r＝[x,y,z]^T的叉乘算子为：

$\tilde{r}=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$

图11为激励轨迹优化求解流程，其中虚线框内的部分体现了自由漂浮空间机器人激励轨迹优化设计的特殊之处。

不同于固定基座机器人，自由漂浮空间机器人动力学参数辨识模型的回归矩阵中不仅包含由机械臂关节运动轨迹直接决定的关节角度和关节角速度，还包含与待辨识动力学参数相关的基座位置、姿态、线速度和角速度。其中，基座的线速度和角速度需要将关节角速度和空间机器人的动力学参数(包括待辨识动力学参数的先验值)代入式(4)进行求解，基座的位置和姿态则需要对基座线速度和角速度进行数值积分获得，因此在激励轨迹优化设计过程中不可避免地要用到待辨识动力学参数的先验信息。

图11中J_i和J_i+1分别为经过i次和i+1次迭代优化后得到的激励轨迹所对应的目标函数值，即回归矩阵A(k)的谱条件数。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (1)

1.一种空间机器人动力学参数辨识的激励轨迹优化方法，其特征在于，包括以下步骤：

1)建立自由漂浮空间机器人的参数辨识模型

空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由n个旋转关节连接而成，所有构件均视为刚体；基座航天器上安装有测量基座位置、姿态、线速度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器；

空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_s\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_m\stackrel{\·}{\mathrm{\φ}}---\left(1\right)$

式中：

$J_s=\left[\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right]\∈R^{6\×6},p_{0e}=p_e-r_0---\left(2\right)$

$J_m=\left[\begin{array}{cccc}{k}_1\×(p_e-p_1)& k_2\×(p_e-p_2)& ...& k_n\×(p_e-p_n)\\ k_1& k_2& ...& k_n\end{array}\right]\∈R^{6\×n}---\left(3\right)$

空间机器人的线动量P和角动量L用矩阵形式统一表示为：

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\\ O& H_{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+\left[\begin{array}{c}{J}_{T\mathrm{\ω}}\\ H_{\mathrm{\ω}\mathrm{\φ}}\end{array}\right]\stackrel{\·}{\mathrm{\φ}}+\left[\begin{array}{c}O\\ r_g\×P\end{array}\right]---\left(4\right)$

式中：

$H_{\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}(I_i+m_i{\tilde{r}}_{gi}^T{\tilde{r}}_{0i})+I_0\∈R^{3\×3}---\left(5\right)$

$H_{\mathrm{\ω}\mathrm{\φ}}=\underset{k=1}{\overset{n}{\Σ}}(I_i{J}_{Ri}+m_i{\tilde{r}}_{gi}{J}_{Ti})\∈R^{3\×n}---\left(6\right)$

$J_{T\mathrm{\ω}}=\underset{i=1}{\overset{n}{\Σ}}{m}_i{J}_{Ti}---\left(7\right)$

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),…,k_i×(r_i-p_i),0,…,0] (8)

J_Ri＝[k₁,k₂,…,k_i,0,…,0] (9)

r_0g＝r_g-r₀,r_gi＝r_i-r_g,r_0i＝r_i-r₀ (10)

设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理得到：

式中：

i_n＝[I_xx,-I_xy,-I_xz,I_yy,-I_yz,I_zz]^T

${\mathrm{\Ω}}_n=R_n^I\left[\begin{array}{cccccc}{\mathrm{\ω}_n}_{nx}& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0& 0& 0\\ 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}& 0\\ 0& 0& {\mathrm{\ω}_n}_{nx}& 0& {\mathrm{\ω}_n}_{ny}& {\mathrm{\ω}_n}_{nz}\end{array}\right]$

$u={\mathrm{\Σ}}_{i=0}^{n-1}{m}_i{\stackrel{\·}{r}}_i$

$q={\mathrm{\Σ}}_{i=0}^{n-1}(I_i{\mathrm{\ω}}_i+r_i\×m_i{\stackrel{\·}{r}}_i)+u\×p_n$

构造一个以为未知量的线性回归方程组进行求解；假设在每个采样点获取一组测量值，当完成第k次采样后，线性回归方程组能够表示如下：

A(k)x＝Y(k) (12)

式中：

$A\left(k\right)=\left[\begin{array}{c}{A}_1\\ A_2\\ .\\ .\\ .\\ A_k\end{array}\right],Y\left(k\right)=\left[\begin{array}{c}{y}_1\\ y_2\\ .\\ .\\ .\\ y_k\end{array}\right]---\left(13\right)$

$A_i={\left[\begin{array}{ccc}u& {\widetilde{\mathrm{\ω}}}_0{R}_n^I+\underset{j=1}{\overset{n}{\Σ}}{\tilde{k}}_j{\stackrel{\·}{\mathrm{\φ}}}_j{R}_n^I& O\\ O& \tilde{u}{R}_n^I& {\mathrm{\Ω}}_n\end{array}\right]}_i,i=1,2,...,k---\left(14\right)$

$y_i=-{\left[\begin{array}{c}{v}_0+{\mathrm{\ω}}_0\×(p_n-r_0)+\underset{j=1}{\overset{n}{\Σ}}(k_j\×(p_n-p_j\left)\right){\stackrel{\·}{\mathrm{\φ}}}_j\\ q\end{array}\right]}_i,i=1,2,...,k---\left(15\right)$

待辨识的动力学参数x可采用最小二乘算法进行估计：

$\hat{x}={\left(A^T\right(k\left)A\right(k\left)\right)}^{-1}{A}^T\left(k\right)Y\left(k\right)---\left(16\right)$

2)确定激励轨迹优化准则

选用回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：

$J={\mathrm{cond}}_2\left(A\right(k\left)\right)=\frac{{\mathrm{\σ}}_{max}\left(A\right(k\left)\right)}{{\mathrm{\σ}}_{\min }\left(A\right(k\left)\right)}---\left(17\right)$

式中：σ_max(A(k))表示矩阵A(k)的最大奇异值，σ_min(A(k))表示矩阵A(k)的最小奇异值；

3)空间机器人关节轨迹参数化

空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：

${\mathrm{\φ}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(\frac{a_l^i}{{\mathrm{\ω}}_{f}l}\sin \right({\mathrm{\ω}}_{f}lt)-\frac{b_l^i}{{\mathrm{\ω}}_{f}l}\cos ({\mathrm{\ω}}_{f}lt\left)\right)+{\mathrm{\φ}}_{i0}---\left(18\right)$

式中：ω_f为傅立叶级数的基频，φ_i0为关节位置偏移量；机械臂各关节采用相同的基频来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2N+1个待定系数；

对式(18)关于时间求一阶导数和二阶导数能够得到关节i的角速度和角加速度如下所示：

${\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)=\underset{l=1}{\overset{N}{\Σ}}\left(a_l^i\cos \right({\mathrm{\ω}}_{f}lt)+b_l^i\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(19\right)$

${\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)={\mathrm{\ω}}_f\underset{l=1}{\overset{N}{\Σ}}\left(b_l^{i}l\cos \right({\mathrm{\ω}}_{f}lt)-a_l^{i}l\sin ({\mathrm{\ω}}_{f}lt\left)\right)---\left(20\right)$

4)激励轨迹优化问题求解

激励轨迹优化问题描述为如下形式：

φ^*(t)＝argmin(J) (21)

约束条件为：

$\left\{\begin{array}{cc}{\mathrm{\φ}}_i\left(t_0\right)={\mathrm{\φ}}_{i0},{\mathrm{\φ}}_i\left(t_f\right)={\mathrm{\φ}}_{if}& \∀i\\ {\stackrel{\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_0\right)=0,{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t_f\right)=0& \∀i\\ {\mathrm{\φ}}_i^{\min }\≤{\mathrm{\φ}}_i\left(t\right)\≤{\mathrm{\φ}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\\ {\stackrel{\·\·}{\mathrm{\φ}}}_i^{\min }\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i\left(t\right)\≤{\stackrel{\·\·}{\mathrm{\φ}}}_i^{\max }& \∀t\∈\[t_0,t_f\],i\end{array}\right.---\left(22\right)$

式(21)和式(22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的fmincon函数求得该非线性优化问题的解，即步骤3)中待定系数和φ_i0的值。