US8504208B2ActiveUtilityA1
Mobile object controller and floor surface estimator
Est. expiryMay 25, 2031(~4.9 yrs left)· nominal 20-yr term from priority
Inventors:Atsuo Orita
G05B 2219/39305B62D 57/032G05B 2219/39182B25J 9/1664
89
PatentIndex Score
11
Cited by
14
References
15
Claims
Abstract
A total floor reaction force required correction amount by which an error between an observed value of a total floor reaction force acting on a mobile object 101 and a desired total floor reaction force approaches zero is converted to a spring displacement amount of a position/posture of a representative contact surface representative of ground surfaces of the mobile object 101 . A correction amount of a displacement amount of each joint of the mobile object 101 is determined by multiplying the spring displacement amount by a pseudo inverse matrix of a Jacobian matrix representing a relation between a change amount of the position and posture of the representative contact surface per unit time and a change amount of a generalized variable vector per unit time. The displacement amount of each joint is controlled according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount.
Claims
exact text as granted — not AI-modifiedWhat is claimed is:
1. A mobile object controller which performs motion control of a mobile object according to a desired motion of the mobile object and a desired total floor reaction force which is a desired value of a total floor reaction force to be applied to the mobile object to realize the desired motion, the mobile object including a body, a plurality of leg links connected to the body, and a joint actuator which drives a joint of each leg link, and moving on a floor surface by movements of the plurality of leg links, the mobile object controller comprising:
a total floor reaction force required correction amount determination processor configured to determine a total floor reaction force required correction amount according to an error between an observed value of the total floor reaction force actually acting on the mobile object and the desired total floor reaction force, the total floor reaction force required correction amount being a required correction amount of the total floor reaction force to be additionally applied to the mobile object so that the error approaches zero;
a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined total floor reaction force required correction amount and a predetermined spring constant of a representative contact surface, a required displacement amount of a position and posture of the representative contact surface corresponding to the total floor reaction force required correction amount based on an assumption that the total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface, the representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the floor surface;
a representative contact surface Jacobian matrix calculation processor configured to calculate a representative contact surface Jacobian matrix Jc according to the following equation (27) from a leg link Jacobian matrix J_i for each leg link, the spring constant, a relative position of an actual floor reaction force central point of a distal end of each leg link relative to a total floor reaction force central point, and a floor reaction force actually acting on each leg link, the representative contact surface Jacobian matrix Jc being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the leg link Jacobian matrix J_i being a Jacobian matrix representing a relation between a temporal change rate of a position of a distal end of the leg link or a temporal change rate of a position and posture of the distal end of the leg link and the temporal change rate of the generalized variable vector, and the total floor reaction force central point being an acting point of the total floor reaction force actually acting on the mobile object;
a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying the calculated required displacement amount of the position and posture of the representative contact surface by a pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of the representative contact surface; and
a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object,
Jc
=
∑
i
=
1
N
(
r_i
·
A_i
·
J_i
)
(
27
)
where
Jc is the representative contact surface Jacobian matrix,
i is an identification number of a leg link,
N is a total number of leg links,
r_i is a weight coefficient of an i-th leg link determined by the following equation (27-1),
r_i
=
Fn_i
/
(
∑
j
=
1
N
Fn_j
)
(
27
-
1
)
Fn_i is a normal force component of a floor reaction force acting on the i-th leg link,
A_i is a matrix defined by the following equation (27-2),
A_i
≡
[
I
0
Rk
·
VV_i
I
]
(
27
-
2
)
Rk is a coefficient matrix defined by the following equation (27-3),
Rk≡Kc — rot −1 ·Kc — org (27-3)
Kc_org is a spring constant matrix relating to a translational displacement of the position of the representative contact surface,
Kc_rot is a spring constant matrix relating to a rotational displacement of the posture of the representative contact surface,
VV_i is a matrix such that VV_i·↑F_i=↑V_i×↑F_i,
↑F_i is a floor reaction force vector acting on the i-th leg link,
↑V_i is a position vector of a floor reaction force central point of a distal end of the i-th leg link relative to the total floor reaction force central point, and
J_i is a leg link Jacobian matrix of the i-th leg link.
2. The mobile object controller according to claim 1 , wherein the total floor reaction force required correction amount determined by the total floor reaction force required correction amount determination processor is a sum total of a proportional term proportional to the error and an integral term obtained by integrating the error.
3. The mobile object controller according to claim 1 , wherein the total floor reaction force required correction amount determined by the total floor reaction force required correction amount determination processor is a value obtained by integrating the error, and
wherein the mobile object controller further comprises
a floor surface estimation processor configured to estimate a position and posture of an actual floor surface by correcting a position and posture of a supposed floor surface according to the required displacement amount calculated by the representative contact surface position/posture displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion.
4. The mobile object controller according to claim 1 , wherein the total floor reaction force required correction amount determined by the total floor reaction force required correction amount determination processor is a value obtained by combining at least a proportional term proportional to the error and an integral term obtained by integrating the error, and
wherein the mobile object controller further comprises:
a representative contact surface steady-state displacement amount calculation processor configured to calculate a representative contact surface steady-state displacement amount from the integral term in the total floor reaction force required correction amount and the spring constant of the representative contact surface, the representative contact surface steady-state displacement amount being a displacement amount of the position and posture of the representative contact surface corresponding to the integral term; and
a floor surface estimation processor configured to estimate a position and posture of an actual floor surface by correcting a position and posture of a supposed floor surface according to the representative contact surface steady-state displacement amount calculated by the representative contact surface steady-state displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion.
5. The mobile object controller according to claim 1 , wherein the pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc is a matrix obtained according to the following equation (30) from a weight matrix W set beforehand and the calculated representative contact surface Jacobian matrix Jc,
wherein the mobile object controller further comprises
a pseudo inverse matrix calculation parameter determination processor configured to determine a value of k in the equation (30) so that a determinant DET expressed by the following equation (31) is equal to or more than a predetermined positive threshold,
Jc −1 =W −1 ·Jc T ·( Jc·W −1 ·Jc T +k·I ) −1 (30)
DET=det ( Jc·W −1 ·Jc T +k·I ) (31)
where W is the weight matrix set beforehand which is a diagonal matrix, and
wherein the pseudo inverse matrix calculation parameter determination processor is configured to: repeatedly perform a process of setting a provisional value of k by gradually increasing the provisional value from a predetermined initial value, calculating the determinant DET using the set provisional value, and determining whether or not an absolute value of the calculated determinant DET is equal to or more than the predetermined threshold, and determine the provisional value of k in the case where a result of the determination is true as the value of k used for calculating the pseudo inverse matrix according to the equation (30); and set an increment of the provisional value of k in the case where the result of the determination is false, to a value proportional to an n-th root of an absolute value of an error between the absolute value of the determinant DET calculated using the provisional value before the increment and the predetermined threshold, where n is an order of Jc·W −1 ·Jc T .
6. A floor surface estimator which estimates a position and posture of an actual floor surface on which a mobile object moves, in a mobile object controller performing motion control of the mobile object according to a desired motion of the mobile object and a desired total floor reaction force which is a desired value of a total floor reaction force to be applied to the mobile object to realize the desired motion, the mobile object including a body, a plurality of leg links connected to the body, and a joint actuator which drives a joint of each leg link, and moving on the floor surface by movements of the plurality of leg links, the floor surface estimator comprising:
a total floor reaction force required correction amount determination processor configured to determine, as a total floor reaction force required correction amount, a result of integrating an error between an observed value of the total floor reaction force actually acting on the mobile object and the desired total floor reaction force, the total floor reaction force required correction amount being a correction amount of the total floor reaction force to be additionally applied to the mobile object so that the error approaches zero;
a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined total floor reaction force required correction amount and a predetermined spring constant of a representative contact surface, a required displacement amount of a position and posture of the representative contact surface corresponding to the total floor reaction force required correction amount based on an assumption that the total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface, the representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the floor surface;
a representative contact surface Jacobian matrix calculation processor configured to calculate a representative contact surface Jacobian matrix Jc according to the following equation (27) from a leg link Jacobian matrix J_i for each leg link, the spring constant, a relative position of an actual floor reaction force central point of a distal end of each leg link relative to a total floor reaction force central point, and a floor reaction force actually acting on each leg link, the representative contact surface Jacobian matrix Jc being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the leg link Jacobian matrix J_i being a Jacobian matrix representing a relation between a temporal change rate of a position of a distal end of the leg link or a temporal change rate of a position and posture of the distal end of the leg link and the temporal change rate of the generalized variable vector, and the total floor reaction force central point being an acting point of the total floor reaction force actually acting on the mobile object;
a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying the calculated required displacement amount of the position and posture of the representative contact surface by a pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of the representative contact surface; and
a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object,
wherein the position and posture of the actual floor surface are estimated by correcting a position and posture of a supposed floor surface according to the required displacement amount calculated by the representative contact surface position/posture displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion,
Jc
=
∑
i
=
1
N
(
r_i
·
A_i
·
J_i
)
(
27
)
where
Jc is the representative contact surface Jacobian matrix,
i is an identification number of a leg link,
N is a total number of leg links,
r_i is a weight coefficient of an i-th leg link determined by the following equation (27-1),
r_i
=
Fn_i
/
(
∑
j
=
1
N
Fn_j
)
(
27
-
1
)
Fn_i is a normal force component of a floor reaction force acting on the i-th leg link,
A_i is a matrix defined by the following equation (27-2),
A_i
≡
[
I
0
Rk
·
VV_i
I
]
(
27
-
2
)
Rk is a coefficient matrix defined by the following equation (27-3),
Rk≡Kc — rot −1 ·Kc — org (27-3)
Kc_org is a spring constant matrix relating to a translational displacement of the position of the representative contact surface,
Kc_rot is a spring constant matrix relating to a rotational displacement of the posture of the representative contact surface,
VV_i is a matrix such that VV_i·↑F_i=↑V_i×↑F_i,
↑F_i is a floor reaction force vector acting on the i-th leg link,
↑V_i is a position vector of a floor reaction force central point of a distal end of the i-th leg link relative to the total floor reaction force central point, and
J_i is a leg link Jacobian matrix of the i-th leg link.
7. A floor surface estimator which estimates a position and posture of an actual floor surface on which a mobile object moves, in a mobile object controller performing motion control of the mobile object according to a desired motion of the mobile object and a desired total floor reaction force which is a desired value of a total floor reaction force to be applied to the mobile object to realize the desired motion, the mobile object including a body, a plurality of leg links connected to the body, and a joint actuator which drives a joint of each leg link, and moving on the floor surface by movements of the plurality of leg links, the floor surface estimator comprising:
a total floor reaction force required correction amount determination processor configured to determine a total floor reaction force required correction amount which is a correction amount of a total floor reaction force to be additionally applied to the mobile object so that an error between an observed value of the total floor reaction force actually acting on the mobile object and the desired total floor reaction force approaches zero, by combining at least a proportional term proportional to the error and an integral term obtained by integrating the error;
a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined total floor reaction force required correction amount and a predetermined spring constant of a representative contact surface, a required displacement amount of a position and posture of the representative contact surface corresponding to the total floor reaction force required correction amount based on an assumption that the total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface, the representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the floor surface;
a representative contact surface Jacobian matrix calculation processor configured to calculate a representative contact surface Jacobian matrix Jc according to the following equation (27) from a leg link Jacobian matrix J_i for each leg link, the spring constant, a relative position of an actual floor reaction force central point of a distal end of each leg link relative to a total floor reaction force central point, and a floor reaction force actually acting on each leg link, the representative contact surface Jacobian matrix Jc being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the leg link Jacobian matrix J_i being a Jacobian matrix representing a relation between a temporal change rate of a position of a distal end of the leg link or a temporal change rate of a position and posture of the distal end of the leg link and the temporal change rate of the generalized variable vector, and the total floor reaction force central point being an acting point of the total floor reaction force actually acting on the mobile object;
a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying the calculated required displacement amount of the position and posture of the representative contact surface by a pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of the representative contact surface;
a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object; and
a representative contact surface steady-state displacement amount calculation processor configured to calculate a representative contact surface steady-state displacement amount from the integral term in the total floor reaction force required correction amount and the spring constant of the representative contact surface, the representative contact surface steady-state displacement amount being a displacement amount of the position and posture of the representative contact surface corresponding to the integral term,
wherein the position and posture of the actual floor surface are estimated by correcting a position and posture of a supposed floor surface according to the representative contact surface steady-state displacement amount calculated by the representative contact surface steady-state displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion,
Jc
=
∑
i
=
1
N
(
r_i
·
A_i
·
J_i
)
(
27
)
where
Jc is the representative contact surface Jacobian matrix,
i is an identification number of a leg link,
N is a total number of leg links,
r_i is a weight coefficient of an i-th leg link determined by the following equation (27-1),
r_i
=
Fn_i
/
(
∑
j
=
1
N
Fn_j
)
(
27
-
1
)
Fn_i is a normal force component of a floor reaction force acting on the i-th leg link,
A_i is a matrix defined by the following equation (27-2),
A_i
≡
[
I
0
Rk
·
VV_i
I
]
(
27
-
2
)
Rk is a coefficient matrix defined by the following equation (27-3),
Rk≡Kc — rot −1 ·Kc — org (27-3)
Kc_org is a spring constant matrix relating to a translational displacement of the position of the representative contact surface,
Kc_rot is a spring constant matrix relating to a rotational displacement of the posture of the representative contact surface,
VV_i is a matrix such that VV_i·↑F_i=↑V_i×↑F_i,
↑F_i is a floor reaction force vector acting on the i-th leg link,
↑V_i is a position vector of a floor reaction force central point of a distal end of the i-th leg link relative to the total floor reaction force central point, and
J_i is a leg link Jacobian matrix of the i-th leg link.
8. The floor surface estimator according to claim 6 , wherein the pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc is a matrix obtained according to the following equation (30) from a weight matrix W set beforehand and the calculated representative contact surface Jacobian matrix Jc,
wherein the floor surface estimator further comprises
a pseudo inverse matrix calculation parameter determination processor configured to determine a value of k in the equation (30) so that a determinant DET expressed by the following equation (31) is equal to or more than a predetermined positive threshold,
Jc −1 =W −1 ·Jc T ·( Jc·W −1 ·Jc T +k·I ) −1 (30)
DET=det ( Jc·W −1 ·Jc T +k·I ) (31)
where W is the weight matrix set beforehand which is a diagonal matrix, and
wherein the pseudo inverse matrix calculation parameter determination processor is configured to: repeatedly perform a process of setting a provisional value of k by gradually increasing the provisional value from a predetermined initial value, calculating the determinant DET using the set provisional value, and determining whether or not an absolute value of the calculated determinant DET is equal to or more than the predetermined threshold, and determine the provisional value of k in the case where a result of the determination is true as the value of k used for calculating the pseudo inverse matrix according to the equation (30); and set an increment of the provisional value of k in the case where the result of the determination is false, to a value proportional to an n-th root of an absolute value of an error between the absolute value of the determinant DET calculated using the provisional value before the increment and the predetermined threshold, where n is an order of Jc·W −1 ·Jc T .
9. The floor surface estimator according to claim 7 , wherein the pseudo inverse matrix Jc −1 of the calculated representative contact surface Jacobian matrix Jc is a matrix obtained according to the following equation (30) from a weight matrix W set beforehand and the calculated representative contact surface Jacobian matrix Jc,
wherein the floor surface estimator further comprises
a pseudo inverse matrix calculation parameter determination processor configured to determine a value of k in the equation (30) so that a determinant DET expressed by the following equation (31) is equal to or more than a predetermined positive threshold,
Jc −1 =W −1 ·Jc T ·( Jc·W −1 ·Jc T +k·I ) −1 (30)
DET=det ( Jc·W −1 ·Jc T +k·I ) (31)
where W is the weight matrix set beforehand which is a diagonal matrix, and
wherein the pseudo inverse matrix calculation parameter determination processor is configured to: repeatedly perform a process of setting a provisional value of k by gradually increasing the provisional value from a predetermined initial value, calculating the determinant DET using the set provisional value, and determining whether or not an absolute value of the calculated determinant DET is equal to or more than the predetermined threshold, and determine the provisional value of k in the case where a result of the determination is true as the value of k used for calculating the pseudo inverse matrix according to the equation (30); and set an increment of the provisional value of k in the case where the result of the determination is false, to a value proportional to an n-th root of an absolute value of an error between the absolute value of the determinant DET calculated using the provisional value before the increment and the predetermined threshold, where n is an order of Jc·W −1 ·Jc T .
10. A mobile object controller which performs motion control of a mobile object according to a desired motion and a desired total contact force, the mobile object including a body, a plurality of movable links connected to the body, and a joint actuator which drives a joint of each movable link, the desired motion being for moving the mobile object while at least one movable link contacts each of first to N-th contact target surfaces which are a plurality of mutually different contact target surfaces existing in a mobile environment of the mobile object where N is an integer equal to or more than 2, and the desired total contact force being a desired value of a total contact force to be applied to the mobile object from each of the first to N-th contact target surfaces to realize the desired motion, the mobile object controller comprising:
a total contact force required correction amount determination processor configured to determine an i-th total contact force required correction amount according to an error between an observed value of a total contact force actually acting on the mobile object from an i-th contact target surface and a desired total contact force corresponding to the i-th contact target surface where i=1, 2, . . . , N, the i-th total contact force required correction amount being a required correction amount of the total contact force to be additionally applied to the mobile object from the i-th contact target surface so that the error approaches zero, the i-th contact target surface being each of the first to N-th contact target surfaces;
a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined i-th total contact force required correction amount and a predetermined spring constant of an i-th representative contact surface corresponding to the i-th contact target surface where i=1, 2, . . . , N, a required displacement amount of a position and posture of the i-th representative contact surface based on an assumption that the i-th total contact force required correction amount is generated by a spring displacement of the position and posture of the i-th representative contact surface, the i-th representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the i-th contact target surface;
a representative contact surface Jacobian matrix calculation processor configured to calculate an i-th representative contact surface Jacobian matrix Jc(i) according to the following equation (77) from a movable link Jacobian matrix J(i)_j for each movable link in an i-th contact movable link group where i=1, 2, . . . , N, the spring constant of the i-th representative contact surface, a relative position of an actual contact force central point of a contact portion of each movable link in the i-th contact movable link group relative to a total contact force central point, and a contact force actually acting on each movable link in the i-th contact movable link group, the i-th representative contact surface Jacobian matrix Jc(i) being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the i-th representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the movable link Jacobian matrix J(i)_j being a Jacobian matrix representing a relation between a temporal change rate of a position of a contact portion of the movable link in the i-th contact movable link group or a temporal change rate of a position and posture of the contact portion of the movable link in the i-th contact movable link group and the temporal change rate of the generalized variable vector, the total contact force central point being an acting point of the total contact force actually acting on the mobile object from the i-th contact target surface, and the i-th contact movable link group being a group of movable links contacting the i-th contact target surface;
a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying an overall required displacement amount by a pseudo inverse matrix Jc −1 of an overall Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of each of the first to N-th representative contact surfaces, the overall required displacement amount being formed by arranging calculated required displacement amounts of positions and postures of first to N-th representative contact surfaces, and the overall Jacobian matrix Jc being formed by arranging calculated first to N-th representative contact surface Jacobian matrices Jc(i) where i=1, 2, . . . , N; and
a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object,
Jc
(
i
)
=
∑
j
=
1
m
(
i
)
(
r
(
i
)
_j
·
A
(
i
)
_j
·
J
(
i
)
_j
)
(
77
)
where
Jc(i) is an i-th representative contact surface Jacobian matrix,
j is an identification number of each movable link belonging to the i-th contact movable link group,
m(i) is a total number of movable links belonging to the i-th contact movable link group,
r(i)_j is a weight coefficient of a j-th movable link in the i-th contact movable link group determined by the following equation (77-1),
r ( i ) — j=Fn ( i ) — j /( j ) (77-1)
Fn(i)_j is a normal force component of a contact force acting on the j-th movable link in the i-th contact movable link group,
A(i)_j is a matrix defined by the following equation (77-2),
A
(
i
)
_j
≡
[
I
0
Rk
(
i
)
·
VV
(
i
)
_j
I
]
(
77
-
2
)
Rk(i) is a coefficient matrix defined by the following equation (77-3),
Rk ( i )= Kc ( i ) — rot −1 ·Kc ( i ) — org (77-3)
Kc(i)_org is a spring constant matrix relating to a translational displacement of the position of the i-th representative contact surface,
Kc(i)_rot is a spring constant matrix relating to a rotational displacement of the posture of the i-th representative contact surface,
VV(i)_j is a matrix such that VV(i)_j·↑F(i)_j=↑V(i)_j×T F(i)_j,
↑F(i)_j is a contact force vector acting on the j-th movable link in the i-th contact movable link group,
↑V(i)_j is a position vector of a contact force central point of a contact portion of the j-th movable link in the i-th contact movable link group relative to the total contact force central point of the i-th contact target surface, and
J(i)_j is a movable link Jacobian matrix of the j-th movable link in the i-th contact movable link group.
11. The mobile object controller according to claim 10 , wherein the total contact force required correction amount determination processor is configured to determine the i-th total contact force required correction amount by integrating the error on the i-th contact target surface where i=1, 2, . . . , N.
12. The mobile object controller according to claim 10 , wherein the total contact force required correction amount determination processor is configured to determine the i-th total contact force required correction amount, by combining at least a proportional term proportional to the error and an integral term obtained by integrating the error on the i-th contact target surface, where i=1, 2, . . . , N.
13. The mobile object controller according to claim 11 , further comprising
a contact target surface estimation processor configured to estimate a position and posture of an actual h-th contact target surface by correcting a position and posture of a supposed h-th contact target surface according to a required displacement amount of an h-th representative contact surface calculated by the representative contact surface position/posture displacement amount calculation processor, the supposed h-th contact target surface being a contact target surface supposed in the desired motion in correspondence with an h-th contact target surface which is a predetermined specific contact target surface of the first to N-th contact target surfaces.
14. The mobile object controller according to claim 12 , further comprising
a contact target surface estimation processor configured to estimate a position and posture of an actual h-th contact target surface by correcting a position and posture of a supposed h-th contact target surface, the supposed h-th contact target surface being a contact target surface supposed in the desired motion in correspondence with an h-th contact target surface which is a predetermined specific contact target surface of the first to N-th contact target surfaces,
wherein the contact target surface estimation processor is configured to estimate the position and posture of the actual h-th contact target surface by: calculating an h-th representative contact surface steady-state displacement amount from an integral term included in an h-th total contact force required correction amount corresponding to the h-th contact target surface and a spring constant of an h-th representative contact surface corresponding to the h-th contact target surface; and correcting the position and posture of the supposed h-th contact target surface according to the h-th representative contact surface steady-state displacement amount, the h-th representative contact surface steady-state displacement amount being a displacement amount of a position and posture of the h-th representative contact surface corresponding to the integral term.
15. The mobile object controller according to claim 10 , wherein the pseudo inverse matrix Jc −1 of the calculated overall Jacobian matrix Jc is a matrix obtained according to the following equation (30) from a weight matrix W set beforehand and the calculated overall Jacobian matrix Jc,
wherein the mobile object controller further comprises
a pseudo inverse matrix calculation parameter determination processor configured to determine a value of k in the equation (30) so that a determinant DET expressed by the following equation (31) is equal to or more than a predetermined positive threshold,
Jc −1 =W −1 ·Jc T ·( Jc·W −1 ·Jc T +k·I ) −1 (30)
DET=det ( Jc·W −1 ·Jc T +k·I ) (31)
where W is the weight matrix set beforehand which is a diagonal matrix, and
wherein the pseudo inverse matrix calculation parameter determination processor is configured to: repeatedly perform a process of setting a provisional value of k by gradually increasing the provisional value from a predetermined initial value, calculating the determinant DET using the set provisional value, and determining whether or not an absolute value of the calculated determinant DET is equal to or more than the predetermined threshold, and determine the provisional value of k in the case where a result of the determination is true as the value of k used for calculating the pseudo inverse matrix Jc −1 according to the equation (30); and set an increment of the provisional value of k in the case where the result of the determination is false, to a value proportional to an n-th root of an absolute value of an error between the absolute value of the determinant DET calculated using the provisional value before the increment and the predetermined threshold, where n is an order of Jc·W −1 ·Jc T .Cited by (0)
No later patents cite this yet.
References (0)
No backward citations on record.